Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
Jacobi.h
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 // This Source Code Form is subject to the terms of the Mozilla 00008 // Public License v. 2.0. If a copy of the MPL was not distributed 00009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00010 00011 #ifndef EIGEN_JACOBI_H 00012 #define EIGEN_JACOBI_H 00013 00014 namespace Eigen { 00015 00016 /** \ingroup Jacobi_Module 00017 * \jacobi_module 00018 * \class JacobiRotation 00019 * \brief Rotation given by a cosine-sine pair. 00020 * 00021 * This class represents a Jacobi or Givens rotation. 00022 * This is a 2D rotation in the plane \c J of angle \f$ \theta \f$ defined by 00023 * its cosine \c c and sine \c s as follow: 00024 * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s & \overline c \end{array} \right ) \f$ 00025 * 00026 * You can apply the respective counter-clockwise rotation to a column vector \c v by 00027 * applying its adjoint on the left: \f$ v = J^* v \f$ that translates to the following Eigen code: 00028 * \code 00029 * v.applyOnTheLeft(J.adjoint()); 00030 * \endcode 00031 * 00032 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() 00033 */ 00034 template<typename Scalar> class JacobiRotation 00035 { 00036 public: 00037 typedef typename NumTraits<Scalar>::Real RealScalar; 00038 00039 /** Default constructor without any initialization. */ 00040 JacobiRotation() {} 00041 00042 /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */ 00043 JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {} 00044 00045 Scalar& c() { return m_c; } 00046 Scalar c() const { return m_c; } 00047 Scalar& s() { return m_s; } 00048 Scalar s() const { return m_s; } 00049 00050 /** Concatenates two planar rotation */ 00051 JacobiRotation operator*(const JacobiRotation & other) 00052 { 00053 using numext::conj; 00054 return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s, 00055 conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c))); 00056 } 00057 00058 /** Returns the transposed transformation */ 00059 JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); } 00060 00061 /** Returns the adjoint transformation */ 00062 JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); } 00063 00064 template<typename Derived> 00065 bool makeJacobi(const MatrixBase<Derived>&, typename Derived::Index p, typename Derived::Index q); 00066 bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z); 00067 00068 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0); 00069 00070 protected: 00071 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type); 00072 void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type); 00073 00074 Scalar m_c, m_s; 00075 }; 00076 00077 /** 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 00078 * \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$ 00079 * 00080 * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() 00081 */ 00082 template<typename Scalar> 00083 bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z) 00084 { 00085 using std::sqrt; 00086 using std::abs; 00087 typedef typename NumTraits<Scalar>::Real RealScalar; 00088 if(y == Scalar(0)) 00089 { 00090 m_c = Scalar(1); 00091 m_s = Scalar(0); 00092 return false; 00093 } 00094 else 00095 { 00096 RealScalar tau = (x-z)/(RealScalar(2)*abs(y)); 00097 RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1)); 00098 RealScalar t; 00099 if(tau>RealScalar(0)) 00100 { 00101 t = RealScalar(1) / (tau + w); 00102 } 00103 else 00104 { 00105 t = RealScalar(1) / (tau - w); 00106 } 00107 RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1); 00108 RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1)); 00109 m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n; 00110 m_c = n; 00111 return true; 00112 } 00113 } 00114 00115 /** 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 00116 * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ (\text{this}_{pq})^* & \text{this}_{qq} \end{array} \right )\f$ yields 00117 * a diagonal matrix \f$ A = J^* B J \f$ 00118 * 00119 * Example: \include Jacobi_makeJacobi.cpp 00120 * Output: \verbinclude Jacobi_makeJacobi.out 00121 * 00122 * \sa JacobiRotation::makeJacobi(RealScalar, Scalar, RealScalar), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() 00123 */ 00124 template<typename Scalar> 00125 template<typename Derived> 00126 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, typename Derived::Index p, typename Derived::Index q) 00127 { 00128 return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q))); 00129 } 00130 00131 /** Makes \c *this as a Givens rotation \c G such that applying \f$ G^* \f$ to the left of the vector 00132 * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields: 00133 * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$. 00134 * 00135 * The value of \a z is returned if \a z is not null (the default is null). 00136 * Also note that G is built such that the cosine is always real. 00137 * 00138 * Example: \include Jacobi_makeGivens.cpp 00139 * Output: \verbinclude Jacobi_makeGivens.out 00140 * 00141 * This function implements the continuous Givens rotation generation algorithm 00142 * found in Anderson (2000), Discontinuous Plane Rotations and the Symmetric Eigenvalue Problem. 00143 * LAPACK Working Note 150, University of Tennessee, UT-CS-00-454, December 4, 2000. 00144 * 00145 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() 00146 */ 00147 template<typename Scalar> 00148 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z) 00149 { 00150 makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type()); 00151 } 00152 00153 00154 // specialization for complexes 00155 template<typename Scalar> 00156 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type) 00157 { 00158 using std::sqrt; 00159 using std::abs; 00160 using numext::conj; 00161 00162 if(q==Scalar(0)) 00163 { 00164 m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1); 00165 m_s = 0; 00166 if(r) *r = m_c * p; 00167 } 00168 else if(p==Scalar(0)) 00169 { 00170 m_c = 0; 00171 m_s = -q/abs(q); 00172 if(r) *r = abs(q); 00173 } 00174 else 00175 { 00176 RealScalar p1 = numext::norm1(p); 00177 RealScalar q1 = numext::norm1(q); 00178 if(p1>=q1) 00179 { 00180 Scalar ps = p / p1; 00181 RealScalar p2 = numext::abs2(ps); 00182 Scalar qs = q / p1; 00183 RealScalar q2 = numext::abs2(qs); 00184 00185 RealScalar u = sqrt(RealScalar(1) + q2/p2); 00186 if(numext::real(p)<RealScalar(0)) 00187 u = -u; 00188 00189 m_c = Scalar(1)/u; 00190 m_s = -qs*conj(ps)*(m_c/p2); 00191 if(r) *r = p * u; 00192 } 00193 else 00194 { 00195 Scalar ps = p / q1; 00196 RealScalar p2 = numext::abs2(ps); 00197 Scalar qs = q / q1; 00198 RealScalar q2 = numext::abs2(qs); 00199 00200 RealScalar u = q1 * sqrt(p2 + q2); 00201 if(numext::real(p)<RealScalar(0)) 00202 u = -u; 00203 00204 p1 = abs(p); 00205 ps = p/p1; 00206 m_c = p1/u; 00207 m_s = -conj(ps) * (q/u); 00208 if(r) *r = ps * u; 00209 } 00210 } 00211 } 00212 00213 // specialization for reals 00214 template<typename Scalar> 00215 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type) 00216 { 00217 using std::sqrt; 00218 using std::abs; 00219 if(q==Scalar(0)) 00220 { 00221 m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1); 00222 m_s = Scalar(0); 00223 if(r) *r = abs(p); 00224 } 00225 else if(p==Scalar(0)) 00226 { 00227 m_c = Scalar(0); 00228 m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1); 00229 if(r) *r = abs(q); 00230 } 00231 else if(abs(p) > abs(q)) 00232 { 00233 Scalar t = q/p; 00234 Scalar u = sqrt(Scalar(1) + numext::abs2(t)); 00235 if(p<Scalar(0)) 00236 u = -u; 00237 m_c = Scalar(1)/u; 00238 m_s = -t * m_c; 00239 if(r) *r = p * u; 00240 } 00241 else 00242 { 00243 Scalar t = p/q; 00244 Scalar u = sqrt(Scalar(1) + numext::abs2(t)); 00245 if(q<Scalar(0)) 00246 u = -u; 00247 m_s = -Scalar(1)/u; 00248 m_c = -t * m_s; 00249 if(r) *r = q * u; 00250 } 00251 00252 } 00253 00254 /**************************************************************************************** 00255 * Implementation of MatrixBase methods 00256 ****************************************************************************************/ 00257 00258 /** \jacobi_module 00259 * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y: 00260 * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$ 00261 * 00262 * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight() 00263 */ 00264 namespace internal { 00265 template<typename VectorX, typename VectorY, typename OtherScalar> 00266 void apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar>& j); 00267 } 00268 00269 /** \jacobi_module 00270 * 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, 00271 * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$. 00272 * 00273 * \sa class JacobiRotation, MatrixBase::applyOnTheRight(), internal::apply_rotation_in_the_plane() 00274 */ 00275 template<typename Derived> 00276 template<typename OtherScalar> 00277 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar> & j) 00278 { 00279 RowXpr x(this->row(p)); 00280 RowXpr y(this->row(q)); 00281 internal::apply_rotation_in_the_plane(x, y, j); 00282 } 00283 00284 /** \ingroup Jacobi_Module 00285 * 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 00286 * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$. 00287 * 00288 * \sa class JacobiRotation, MatrixBase::applyOnTheLeft(), internal::apply_rotation_in_the_plane() 00289 */ 00290 template<typename Derived> 00291 template<typename OtherScalar> 00292 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar> & j) 00293 { 00294 ColXpr x(this->col(p)); 00295 ColXpr y(this->col(q)); 00296 internal::apply_rotation_in_the_plane(x, y, j.transpose()); 00297 } 00298 00299 namespace internal { 00300 template<typename VectorX, typename VectorY, typename OtherScalar> 00301 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<OtherScalar> & j) 00302 { 00303 typedef typename VectorX::Index Index; 00304 typedef typename VectorX::Scalar Scalar; 00305 enum { PacketSize = packet_traits<Scalar>::size }; 00306 typedef typename packet_traits<Scalar>::type Packet; 00307 eigen_assert(_x.size() == _y.size()); 00308 Index size = _x.size(); 00309 Index incrx = _x.innerStride(); 00310 Index incry = _y.innerStride(); 00311 00312 Scalar* EIGEN_RESTRICT x = &_x.coeffRef(0); 00313 Scalar* EIGEN_RESTRICT y = &_y.coeffRef(0); 00314 00315 OtherScalar c = j.c(); 00316 OtherScalar s = j.s(); 00317 if (c==OtherScalar(1) && s==OtherScalar(0)) 00318 return; 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 = internal::first_aligned(y, size); 00330 Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize; 00331 00332 const Packet pc = pset1<Packet>(c); 00333 const Packet ps = pset1<Packet>(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] = c * xi + numext::conj(s) * yi; 00341 y[i] = -s * xi + numext::conj(c) * yi; 00342 } 00343 00344 Scalar* EIGEN_RESTRICT px = x + alignedStart; 00345 Scalar* EIGEN_RESTRICT py = y + alignedStart; 00346 00347 if(internal::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] = c * xi + numext::conj(s) * yi; 00389 y[i] = -s * xi + numext::conj(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>(c); 00399 const Packet ps = pset1<Packet>(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 = c * xi + numext::conj(s) * yi; 00422 *y = -s * xi + numext::conj(c) * yi; 00423 x += incrx; 00424 y += incry; 00425 } 00426 } 00427 } 00428 00429 } // end namespace internal 00430 00431 } // end namespace Eigen 00432 00433 #endif // EIGEN_JACOBI_H
Generated on Tue Jul 12 2022 17:46:56 by 1.7.2