Main MRPT website > C++ reference
MRPT logo

Jacobi.h

Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
00005 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
00006 //
00007 // Eigen is free software; you can redistribute it and/or
00008 // modify it under the terms of the GNU Lesser General Public
00009 // License as published by the Free Software Foundation; either
00010 // version 3 of the License, or (at your option) any later version.
00011 //
00012 // Alternatively, you can redistribute it and/or
00013 // modify it under the terms of the GNU General Public License as
00014 // published by the Free Software Foundation; either version 2 of
00015 // the License, or (at your option) any later version.
00016 //
00017 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00018 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00019 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00020 // GNU General Public License for more details.
00021 //
00022 // You should have received a copy of the GNU Lesser General Public
00023 // License and a copy of the GNU General Public License along with
00024 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00025 
00026 #ifndef EIGEN_JACOBI_H
00027 #define EIGEN_JACOBI_H
00028 
00029 /** \ingroup Jacobi_Module
00030   * \jacobi_module
00031   * \class JacobiRotation
00032   * \brief Rotation given by a cosine-sine pair.
00033   *
00034   * This class represents a Jacobi or Givens rotation.
00035   * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by
00036   * its cosine \c c and sine \c s as follow:
00037   * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s  & \overline c \end{array} \right ) \f$
00038   *
00039   * You can apply the respective counter-clockwise rotation to a column vector \c v by
00040   * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code:
00041   * \code
00042   * v.applyOnTheLeft(J.adjoint());
00043   * \endcode
00044   *
00045   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00046   */
00047 template<typename Scalar> class JacobiRotation
00048 {
00049   public:
00050     typedef typename NumTraits<Scalar>::Real RealScalar;
00051 
00052     /** Default constructor without any initialization. */
00053     JacobiRotation() {}
00054 
00055     /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
00056     JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
00057 
00058     Scalar& c() { return m_c; }
00059     Scalar c() const { return m_c; }
00060     Scalar& s() { return m_s; }
00061     Scalar s() const { return m_s; }
00062 
00063     /** Concatenates two planar rotation */
00064     JacobiRotation operator*(const JacobiRotation& other)
00065     {
00066       return JacobiRotation(m_c * other.m_c - internal::conj(m_s) * other.m_s,
00067                             internal::conj(m_c * internal::conj(other.m_s) + internal::conj(m_s) * internal::conj(other.m_c)));
00068     }
00069 
00070     /** Returns the transposed transformation */
00071     JacobiRotation transpose() const { return JacobiRotation(m_c, -internal::conj(m_s)); }
00072 
00073     /** Returns the adjoint transformation */
00074     JacobiRotation adjoint() const { return JacobiRotation(internal::conj(m_c), -m_s); }
00075 
00076     template<typename Derived>
00077     bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q);
00078     bool makeJacobi(RealScalar x, Scalar y, RealScalar z);
00079 
00080     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
00081 
00082   protected:
00083     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
00084     void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
00085 
00086     Scalar m_c, m_s;
00087 };
00088 
00089 /** Makes \c *this as a Jacobi rotation \a J such that applying \a J on both the right and left sides of the selfadjoint 2x2 matrix
00090   * \f$ B = \left ( \begin{array}{cc} x & y \\ \overline y & z \end{array} \right )\f$ yields a diagonal matrix \f$ A = J^* B J \f$
00091   *
00092   * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00093   */
00094 template<typename Scalar>
00095 bool JacobiRotation<Scalar>::makeJacobi(RealScalar x, Scalar y, RealScalar z)
00096 {
00097   typedef typename NumTraits<Scalar>::Real RealScalar;
00098   if(y == Scalar(0))
00099   {
00100     m_c = Scalar(1);
00101     m_s = Scalar(0);
00102     return false;
00103   }
00104   else
00105   {
00106     RealScalar tau = (x-z)/(RealScalar(2)*internal::abs(y));
00107     RealScalar w = internal::sqrt(internal::abs2(tau) + 1);
00108     RealScalar t;
00109     if(tau>0)
00110     {
00111       t = RealScalar(1) / (tau + w);
00112     }
00113     else
00114     {
00115       t = RealScalar(1) / (tau - w);
00116     }
00117     RealScalar sign_t = t > 0 ? 1 : -1;
00118     RealScalar n = RealScalar(1) / internal::sqrt(internal::abs2(t)+1);
00119     m_s = - sign_t * (internal::conj(y) / internal::abs(y)) * internal::abs(t) * n;
00120     m_c = n;
00121     return true;
00122   }
00123 }
00124 
00125 /** Makes \c *this as a Jacobi rotation \c J such that applying \a J on both the right and left sides of the 2x2 selfadjoint matrix
00126   * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields
00127   * a diagonal matrix \f$ A = J^* B J \f$
00128   *
00129   * Example: \include Jacobi_makeJacobi.cpp
00130   * Output: \verbinclude Jacobi_makeJacobi.out
00131   *
00132   * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00133   */
00134 template<typename Scalar>
00135 template<typename Derived>
00136 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q)
00137 {
00138   return makeJacobi(internal::real(m.coeff(p,p)), m.coeff(p,q), internal::real(m.coeff(q,q)));
00139 }
00140 
00141 /** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector
00142   * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
00143   * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
00144   *
00145   * The value of \a z is returned if \a z is not null (the default is null).
00146   * Also note that G is built such that the cosine is always real.
00147   *
00148   * Example: \include Jacobi_makeGivens.cpp
00149   * Output: \verbinclude Jacobi_makeGivens.out
00150   *
00151   * This function implements the continuous Givens rotation generation algorithm
00152   * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem.
00153   * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000.
00154   *
00155   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00156   */
00157 template<typename Scalar>
00158 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
00159 {
00160   makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
00161 }
00162 
00163 
00164 // specialization for complexes
00165 template<typename Scalar>
00166 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
00167 {
00168   if(q==Scalar(0))
00169   {
00170     m_c = internal::real(p)<0 ? Scalar(-1) : Scalar(1);
00171     m_s = 0;
00172     if(r) *r = m_c * p;
00173   }
00174   else if(p==Scalar(0))
00175   {
00176     m_c = 0;
00177     m_s = -q/internal::abs(q);
00178     if(r) *r = internal::abs(q);
00179   }
00180   else
00181   {
00182     RealScalar p1 = internal::norm1(p);
00183     RealScalar q1 = internal::norm1(q);
00184     if(p1>=q1)
00185     {
00186       Scalar ps = p / p1;
00187       RealScalar p2 = internal::abs2(ps);
00188       Scalar qs = q / p1;
00189       RealScalar q2 = internal::abs2(qs);
00190 
00191       RealScalar u = internal::sqrt(RealScalar(1) + q2/p2);
00192       if(internal::real(p)<RealScalar(0))
00193         u = -u;
00194 
00195       m_c = Scalar(1)/u;
00196       m_s = -qs*internal::conj(ps)*(m_c/p2);
00197       if(r) *r = p * u;
00198     }
00199     else
00200     {
00201       Scalar ps = p / q1;
00202       RealScalar p2 = internal::abs2(ps);
00203       Scalar qs = q / q1;
00204       RealScalar q2 = internal::abs2(qs);
00205 
00206       RealScalar u = q1 * internal::sqrt(p2 + q2);
00207       if(internal::real(p)<RealScalar(0))
00208         u = -u;
00209 
00210       p1 = internal::abs(p);
00211       ps = p/p1;
00212       m_c = p1/u;
00213       m_s = -internal::conj(ps) * (q/u);
00214       if(r) *r = ps * u;
00215     }
00216   }
00217 }
00218 
00219 // specialization for reals
00220 template<typename Scalar>
00221 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
00222 {
00223 
00224   if(q==0)
00225   {
00226     m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
00227     m_s = 0;
00228     if(r) *r = internal::abs(p);
00229   }
00230   else if(p==0)
00231   {
00232     m_c = 0;
00233     m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
00234     if(r) *r = internal::abs(q);
00235   }
00236   else if(internal::abs(p) > internal::abs(q))
00237   {
00238     Scalar t = q/p;
00239     Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00240     if(p<Scalar(0))
00241       u = -u;
00242     m_c = Scalar(1)/u;
00243     m_s = -t * m_c;
00244     if(r) *r = p * u;
00245   }
00246   else
00247   {
00248     Scalar t = p/q;
00249     Scalar u = internal::sqrt(Scalar(1) + internal::abs2(t));
00250     if(q<Scalar(0))
00251       u = -u;
00252     m_s = -Scalar(1)/u;
00253     m_c = -t * m_s;
00254     if(r) *r = q * u;
00255   }
00256 
00257 }
00258 
00259 /****************************************************************************************
00260 *   Implementation of MatrixBase methods
00261 ****************************************************************************************/
00262 
00263 /** \jacobi_module
00264   * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y:
00265   * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right )  =  J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
00266   *
00267   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
00268   */
00269 namespace internal {
00270 template<typename VectorX, typename VectorY, typename OtherScalar>
00271 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j);
00272 }
00273 
00274 /** \jacobi_module
00275   * Applies the rotation in the plane \a j to the rows \a p and \a q of \c *this, i.e., it computes B = J * B,
00276   * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
00277   *
00278   * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane()
00279   */
00280 template<typename Derived>
00281 template<typename OtherScalar>
00282 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00283 {
00284   RowXpr x(this->row(p));
00285   RowXpr y(this->row(q));
00286   internal::apply_rotation_in_the_plane(x, y, j);
00287 }
00288 
00289 /** \ingroup Jacobi_Module
00290   * Applies the rotation in the plane \a j to the columns \a p and \a q of \c *this, i.e., it computes B = B * J
00291   * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
00292   *
00293   * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane()
00294   */
00295 template<typename Derived>
00296 template<typename OtherScalar>
00297 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
00298 {
00299   ColXpr x(this->col(p));
00300   ColXpr y(this->col(q));
00301   internal::apply_rotation_in_the_plane(x, y, j.transpose());
00302 }
00303 
00304 namespace internal {
00305 template<typename VectorX, typename VectorY, typename OtherScalar>
00306 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j)
00307 {
00308   typedef typename VectorX::Index Index;
00309   typedef typename VectorX::Scalar Scalar;
00310   enum { PacketSize = packet_traits<Scalar>::size };
00311   typedef typename packet_traits<Scalar>::type Packet;
00312   eigen_assert(_x.size() == _y.size());
00313   Index size = _x.size();
00314   Index incrx = _x.innerStride();
00315   Index incry = _y.innerStride();
00316 
00317   Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0);
00318   Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0);
00319 
00320   /*** dynamic-size vectorized paths ***/
00321 
00322   if(VectorX::SizeAtCompileTime == Dynamic &&
00323     (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00324     ((incrx==1 && incry==1) || PacketSize == 1))
00325   {
00326     // both vectors are sequentially stored in memory => vectorization
00327     enum { Peeling = 2 };
00328 
00329     Index alignedStart = first_aligned(y, size);
00330     Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
00331 
00332     const Packet pc = pset1<Packet>(j.c());
00333     const Packet ps = pset1<Packet>(j.s());
00334     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00335 
00336     for(Index i=0; i<alignedStart; ++i)
00337     {
00338       Scalar xi = x[i];
00339       Scalar yi = y[i];
00340       x[i] =  j.c() * xi + conj(j.s()) * yi;
00341       y[i] = -j.s() * xi + conj(j.c()) * yi;
00342     }
00343 
00344     Scalar* EIGEN_RESTRICT px = x + alignedStart;
00345     Scalar* EIGEN_RESTRICT py = y + alignedStart;
00346 
00347     if(first_aligned(x, size)==alignedStart)
00348     {
00349       for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
00350       {
00351         Packet xi = pload<Packet>(px);
00352         Packet yi = pload<Packet>(py);
00353         pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00354         pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00355         px += PacketSize;
00356         py += PacketSize;
00357       }
00358     }
00359     else
00360     {
00361       Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
00362       for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
00363       {
00364         Packet xi   = ploadu<Packet>(px);
00365         Packet xi1  = ploadu<Packet>(px+PacketSize);
00366         Packet yi   = pload <Packet>(py);
00367         Packet yi1  = pload <Packet>(py+PacketSize);
00368         pstoreu(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00369         pstoreu(px+PacketSize, padd(pmul(pc,xi1),pcj.pmul(ps,yi1)));
00370         pstore (py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00371         pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pmul(ps,xi1)));
00372         px += Peeling*PacketSize;
00373         py += Peeling*PacketSize;
00374       }
00375       if(alignedEnd!=peelingEnd)
00376       {
00377         Packet xi = ploadu<Packet>(x+peelingEnd);
00378         Packet yi = pload <Packet>(y+peelingEnd);
00379         pstoreu(x+peelingEnd, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00380         pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00381       }
00382     }
00383 
00384     for(Index i=alignedEnd; i<size; ++i)
00385     {
00386       Scalar xi = x[i];
00387       Scalar yi = y[i];
00388       x[i] =  j.c() * xi + conj(j.s()) * yi;
00389       y[i] = -j.s() * xi + conj(j.c()) * yi;
00390     }
00391   }
00392 
00393   /*** fixed-size vectorized path ***/
00394   else if(VectorX::SizeAtCompileTime != Dynamic &&
00395           (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
00396           (VectorX::Flags & VectorY::Flags & AlignedBit))
00397   {
00398     const Packet pc = pset1<Packet>(j.c());
00399     const Packet ps = pset1<Packet>(j.s());
00400     conj_helper<Packet,Packet,NumTraits<Scalar>::IsComplex,false> pcj;
00401     Scalar* EIGEN_RESTRICT px = x;
00402     Scalar* EIGEN_RESTRICT py = y;
00403     for(Index i=0; i<size; i+=PacketSize)
00404     {
00405       Packet xi = pload<Packet>(px);
00406       Packet yi = pload<Packet>(py);
00407       pstore(px, padd(pmul(pc,xi),pcj.pmul(ps,yi)));
00408       pstore(py, psub(pcj.pmul(pc,yi),pmul(ps,xi)));
00409       px += PacketSize;
00410       py += PacketSize;
00411     }
00412   }
00413 
00414   /*** non-vectorized path ***/
00415   else
00416   {
00417     for(Index i=0; i<size; ++i)
00418     {
00419       Scalar xi = *x;
00420       Scalar yi = *y;
00421       *x =  j.c() * xi + conj(j.s()) * yi;
00422       *y = -j.s() * xi + conj(j.c()) * yi;
00423       x += incrx;
00424       y += incry;
00425     }
00426   }
00427 }
00428 }
00429 
00430 #endif // EIGEN_JACOBI_H



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011