diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h index 9b1bf9e19..c2945ab46 100644 --- a/Eigen/src/Core/MatrixBase.h +++ b/Eigen/src/Core/MatrixBase.h @@ -804,10 +804,10 @@ template class MatrixBase ///////// Jacobi module ///////// template - void applyJacobiOnTheLeft(int p, int q, JacobiScalar c, JacobiScalar s); + void applyJacobiOnTheLeft(int p, int q, const JacobiRotation& j); template - void applyJacobiOnTheRight(int p, int q, JacobiScalar c, JacobiScalar s); - bool makeJacobi(int p, int q, Scalar *c, Scalar *s) const; + void applyJacobiOnTheRight(int p, int q, const JacobiRotation& j); + bool makeJacobi(int p, int q, JacobiRotation *j) const; #ifdef EIGEN_MATRIXBASE_PLUGIN #include EIGEN_MATRIXBASE_PLUGIN diff --git a/Eigen/src/Core/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h index d7dc61e73..18d3af7c5 100644 --- a/Eigen/src/Core/util/ForwardDeclarations.h +++ b/Eigen/src/Core/util/ForwardDeclarations.h @@ -123,6 +123,7 @@ template class SVD; template class JacobiSVD; template class LLT; template class LDLT; +template class JacobiRotation; // Geometry module: template class RotationBase; diff --git a/Eigen/src/Jacobi/Jacobi.h b/Eigen/src/Jacobi/Jacobi.h index 96f08d54a..24fb7e782 100644 --- a/Eigen/src/Jacobi/Jacobi.h +++ b/Eigen/src/Jacobi/Jacobi.h @@ -26,56 +26,98 @@ #ifndef EIGEN_JACOBI_H #define EIGEN_JACOBI_H -/** Applies the counter clock wise 2D rotation of angle \c theta given by its - * cosine \a c and sine \a s to the set of 2D vectors of cordinates \a x and \a y: - * \f$ x = c x - s' y \f$ - * \f$ y = s x + c y \f$ +/** \ingroup Jacobi + * \class JacobiRotation + * \brief Represents a rotation in the plane from a cosine-sine pair. + * + * This class represents a Jacobi rotation which is also known as a Givens rotation. + * This is a 2D clock-wise rotation in the plane \c J of angle \f$ \theta \f$ defined by + * its cosine \c c and sine \c s as follow: + * \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s & \overline c \end{array} \right ) \f$ + * + * \sa MatrixBase::makeJacobi(), MatrixBase::applyJacobiOnTheLeft(), MatrixBase::applyJacobiOnTheRight() + */ +template class JacobiRotation +{ + public: + /** Default constructor without any initialization. */ + JacobiRotation() {} + + /** Construct a Jacobi rotation from a cosine-sine pair (\a c, \c s). */ + JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {} + + Scalar& c() { return m_c; } + Scalar c() const { return m_c; } + Scalar& s() { return m_s; } + Scalar s() const { return m_s; } + + /** Concatenates two Jacobi rotation */ + JacobiRotation operator*(const JacobiRotation& other) + { + return JacobiRotation(m_c * other.m_c - ei_conj(m_s) * other.m_s, + ei_conj(m_c * ei_conj(other.m_s) + ei_conj(m_s) * ei_conj(other.m_c))); + } + + /** Returns the transposed transformation */ + JacobiRotation transpose() const { return JacobiRotation(m_c, -ei_conj(m_s)); } + + /** Returns the adjoint transformation */ + JacobiRotation adjoint() const { return JacobiRotation(ei_conj(m_c), -m_s); } + + protected: + Scalar m_c, m_s; +}; + +/** Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y: + * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$ * * \sa MatrixBase::applyJacobiOnTheLeft(), MatrixBase::applyJacobiOnTheRight() */ template -void ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, JacobiScalar c, JacobiScalar s); +void ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation& j); -/** Applies a rotation in the plane defined by \a c, \a s to the rows \a p and \a q of \c *this. - * More precisely, it computes B = J' * B, with J = [c s ; -s' c] and B = [ *this.row(p) ; *this.row(q) ] - * \sa MatrixBase::applyJacobiOnTheRight(), ei_apply_rotation_in_the_plane() +/** 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, + * with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$. + * + * \sa class JacobiRotation, MatrixBase::applyJacobiOnTheRight(), ei_apply_rotation_in_the_plane() */ template template -inline void MatrixBase::applyJacobiOnTheLeft(int p, int q, JacobiScalar c, JacobiScalar s) +inline void MatrixBase::applyJacobiOnTheLeft(int p, int q, const JacobiRotation& j) { RowXpr x(row(p)); RowXpr y(row(q)); - ei_apply_rotation_in_the_plane(x, y, c, s); + ei_apply_rotation_in_the_plane(x, y, j); } -/** Applies a rotation in the plane defined by \a c, \a s to the columns \a p and \a q of \c *this. - * More precisely, it computes B = B * J, with J = [c s ; -s' c] and B = [ *this.col(p) ; *this.col(q) ] - * \sa MatrixBase::applyJacobiOnTheLeft(), ei_apply_rotation_in_the_plane() +/** 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 + * with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$. + * + * \sa class JacobiRotation, MatrixBase::applyJacobiOnTheLeft(), ei_apply_rotation_in_the_plane() */ template template -inline void MatrixBase::applyJacobiOnTheRight(int p, int q, JacobiScalar c, JacobiScalar s) +inline void MatrixBase::applyJacobiOnTheRight(int p, int q, const JacobiRotation& j) { ColXpr x(col(p)); ColXpr y(col(q)); - ei_apply_rotation_in_the_plane(x, y, c, -ei_conj(s)); + ei_apply_rotation_in_the_plane(x, y, j.transpose()); } -/** Computes the cosine-sine pair (\a c, \a s) such that its associated - * rotation \f$ J = ( \begin{array}{cc} c & \overline s \\ -s & \overline c \end{array} )\f$ - * applied to both the right and left of the 2x2 matrix - * \f$ B = ( \begin{array}{cc} x & y \\ * & z \end{array} )\f$ yields - * a diagonal matrix A: \f$ A = J^* B J \f$ +/** Computes the Jacobi rotation \a J such that applying \a J on both the right and left sides of the 2x2 matrix + * \f$ B = \left ( \begin{array}{cc} x & y \\ * & z \end{array} \right )\f$ yields + * a diagonal matrix \f$ A = J^* B J \f$ + * + * \sa MatrixBase::makeJacobi(), MatrixBase::applyJacobiOnTheLeft(), MatrixBase::applyJacobiOnTheRight() */ template -bool ei_makeJacobi(typename NumTraits::Real x, Scalar y, typename NumTraits::Real z, Scalar *c, Scalar *s) +bool ei_makeJacobi(typename NumTraits::Real x, Scalar y, typename NumTraits::Real z, JacobiRotation *j) { typedef typename NumTraits::Real RealScalar; if(y == Scalar(0)) { - *c = Scalar(1); - *s = Scalar(0); + j->c() = Scalar(1); + j->s() = Scalar(0); return false; } else @@ -93,20 +135,26 @@ bool ei_makeJacobi(typename NumTraits::Real x, Scalar y, typename NumTra } RealScalar sign_t = t > 0 ? 1 : -1; RealScalar n = RealScalar(1) / ei_sqrt(ei_abs2(t)+1); - *s = - sign_t * (ei_conj(y) / ei_abs(y)) * ei_abs(t) * n; - *c = n; + j->s() = - sign_t * (ei_conj(y) / ei_abs(y)) * ei_abs(t) * n; + j->c() = n; return true; } } +/** Computes the Jacobi rotation \a J such that applying \a J on both the right and left sides of the 2x2 matrix + * \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ * & \text{this}_{qq} \end{array} \right )\f$ yields + * a diagonal matrix \f$ A = J^* B J \f$ + * + * \sa MatrixBase::ei_make_jacobi(), MatrixBase::applyJacobiOnTheLeft(), MatrixBase::applyJacobiOnTheRight() + */ template -inline bool MatrixBase::makeJacobi(int p, int q, Scalar *c, Scalar *s) const +inline bool MatrixBase::makeJacobi(int p, int q, JacobiRotation *j) const { - return ei_makeJacobi(ei_real(coeff(p,p)), coeff(p,q), ei_real(coeff(q,q)), c, s); + return ei_makeJacobi(ei_real(coeff(p,p)), coeff(p,q), ei_real(coeff(q,q)), j); } template -void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, JacobiScalar c, JacobiScalar s) +void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation& j) { typedef typename VectorX::Scalar Scalar; ei_assert(_x.size() == _y.size()); @@ -126,16 +174,16 @@ void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& int alignedStart = ei_alignmentOffset(y, size); int alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize; - const Packet pc = ei_pset1(Scalar(c)); - const Packet ps = ei_pset1(Scalar(s)); + const Packet pc = ei_pset1(Scalar(j.c())); + const Packet ps = ei_pset1(Scalar(j.s())); ei_conj_helper::IsComplex,false> cj; for(int i=0; i class JacobiSVD MaxDiagSizeAtCompileTime = EIGEN_ENUM_MIN(MaxRowsAtCompileTime,MaxColsAtCompileTime), MatrixOptions = MatrixType::Options }; - + typedef Matrix DummyMatrixType; typedef typename ei_meta_if class JacobiSVD JacobiSVD() : m_isInitialized(false) {} - JacobiSVD(const MatrixType& matrix) : m_isInitialized(false) + JacobiSVD(const MatrixType& matrix) : m_isInitialized(false) { compute(matrix); } - + JacobiSVD& compute(const MatrixType& matrix); - + const MatrixUType& matrixU() const { ei_assert(m_isInitialized && "JacobiSVD is not initialized."); @@ -103,7 +103,7 @@ template class JacobiSVD MatrixVType m_matrixV; SingularValuesType m_singularValues; bool m_isInitialized; - + template friend struct ei_svd_precondition_2x2_block_to_be_real; }; @@ -120,11 +120,12 @@ struct ei_svd_precondition_2x2_block_to_be_real typedef JacobiSVD SVD; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; - + enum { ComputeU = SVD::ComputeU, ComputeV = SVD::ComputeV }; static void run(MatrixType& work_matrix, JacobiSVD& svd, int p, int q) { - Scalar c, s, z; + Scalar z; + JacobiRotation rot; RealScalar n = ei_sqrt(ei_abs2(work_matrix.coeff(p,p)) + ei_abs2(work_matrix.coeff(q,p))); if(n==0) { @@ -137,10 +138,10 @@ struct ei_svd_precondition_2x2_block_to_be_real } else { - c = ei_conj(work_matrix.coeff(p,p)) / n; - s = work_matrix.coeff(q,p) / n; - work_matrix.applyJacobiOnTheLeft(p,q,c,s); - if(ComputeU) svd.m_matrixU.applyJacobiOnTheRight(p,q,ei_conj(c),-s); + rot.c() = ei_conj(work_matrix.coeff(p,p)) / n; + rot.s() = work_matrix.coeff(q,p) / n; + work_matrix.applyJacobiOnTheLeft(p,q,rot); + if(ComputeU) svd.m_matrixU.applyJacobiOnTheRight(p,q,rot.adjoint()); if(work_matrix.coeff(p,q) != Scalar(0)) { Scalar z = ei_abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); @@ -154,38 +155,34 @@ struct ei_svd_precondition_2x2_block_to_be_real if(ComputeU) svd.m_matrixU.col(q) *= ei_conj(z); } } - } + } }; template void ei_real_2x2_jacobi_svd(const MatrixType& matrix, int p, int q, - RealScalar *c_left, RealScalar *s_left, - RealScalar *c_right, RealScalar *s_right) + JacobiRotation *j_left, + JacobiRotation *j_right) { Matrix m; m << ei_real(matrix.coeff(p,p)), ei_real(matrix.coeff(p,q)), - ei_real(matrix.coeff(q,p)), ei_real(matrix.coeff(q,q)); - RealScalar c1, s1; + ei_real(matrix.coeff(q,p)), ei_real(matrix.coeff(q,q)); + JacobiRotation rot1; RealScalar t = m.coeff(0,0) + m.coeff(1,1); RealScalar d = m.coeff(1,0) - m.coeff(0,1); if(t == RealScalar(0)) { - c1 = 0; - s1 = d > 0 ? 1 : -1; + rot1.c() = 0; + rot1.s() = d > 0 ? 1 : -1; } else { RealScalar u = d / t; - c1 = RealScalar(1) / ei_sqrt(1 + ei_abs2(u)); - s1 = c1 * u; + rot1.c() = RealScalar(1) / ei_sqrt(1 + ei_abs2(u)); + rot1.s() = rot1.c() * u; } - m.applyJacobiOnTheLeft(0,1,c1,s1); - RealScalar c2, s2; - m.makeJacobi(0,1,&c2,&s2); - *c_left = c1*c2 + s1*s2; - *s_left = s1*c2 - c1*s2; - *c_right = c2; - *s_right = s2; + m.applyJacobiOnTheLeft(0,1,rot1); + m.makeJacobi(0,1,j_right); + *j_left = rot1 * j_right->transpose(); } template @@ -208,18 +205,18 @@ sweep_again: { ei_svd_precondition_2x2_block_to_be_real::run(work_matrix, *this, p, q); - RealScalar c_left, s_left, c_right, s_right; - ei_real_2x2_jacobi_svd(work_matrix, p, q, &c_left, &s_left, &c_right, &s_right); - - work_matrix.applyJacobiOnTheLeft(p,q,c_left,s_left); - if(ComputeU) m_matrixU.applyJacobiOnTheRight(p,q,c_left,-s_left); - - work_matrix.applyJacobiOnTheRight(p,q,c_right,s_right); - if(ComputeV) m_matrixV.applyJacobiOnTheRight(p,q,c_right,s_right); + JacobiRotation j_left, j_right; + ei_real_2x2_jacobi_svd(work_matrix, p, q, &j_left, &j_right); + + work_matrix.applyJacobiOnTheLeft(p,q,j_left); + if(ComputeU) m_matrixU.applyJacobiOnTheRight(p,q,j_left.transpose()); + + work_matrix.applyJacobiOnTheRight(p,q,j_right); + if(ComputeV) m_matrixV.applyJacobiOnTheRight(p,q,j_right); } } } - + RealScalar biggestOnDiag = work_matrix.diagonal().cwise().abs().maxCoeff(); RealScalar maxAllowedOffDiag = biggestOnDiag * precision; for(int p = 0; p < size; ++p) @@ -231,7 +228,7 @@ sweep_again: if(ei_abs(work_matrix.coeff(p,q)) > maxAllowedOffDiag) goto sweep_again; } - + for(int i = 0; i < size; ++i) { RealScalar a = ei_abs(work_matrix.coeff(i,i)); @@ -251,7 +248,7 @@ sweep_again: if(ComputeV) m_matrixV.col(pos).swap(m_matrixV.col(i)); } } - + m_isInitialized = true; return *this; }