diff options
| -rw-r--r-- | Eigen/src/Core/MatrixBase.h | 6 | ||||
| -rw-r--r-- | Eigen/src/Core/util/ForwardDeclarations.h | 1 | ||||
| -rw-r--r-- | Eigen/src/Jacobi/Jacobi.h | 120 | ||||
| -rw-r--r-- | Eigen/src/SVD/JacobiSVD.h | 73 |
4 files changed, 123 insertions, 77 deletions
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<typename Derived> class MatrixBase ///////// Jacobi module ///////// template<typename JacobiScalar> - void applyJacobiOnTheLeft(int p, int q, JacobiScalar c, JacobiScalar s); + void applyJacobiOnTheLeft(int p, int q, const JacobiRotation<JacobiScalar>& j); template<typename JacobiScalar> - 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<JacobiScalar>& j); + bool makeJacobi(int p, int q, JacobiRotation<Scalar> *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<typename MatrixType> class SVD; template<typename MatrixType, unsigned int Options = 0> class JacobiSVD; template<typename MatrixType, int UpLo = LowerTriangular> class LLT; template<typename MatrixType> class LDLT; +template<typename Scalar> class JacobiRotation; // Geometry module: template<typename Derived, int _Dim> 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<typename Scalar> 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<typename VectorX, typename VectorY, typename JacobiScalar> -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<JacobiScalar>& 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<typename Derived> template<typename JacobiScalar> -inline void MatrixBase<Derived>::applyJacobiOnTheLeft(int p, int q, JacobiScalar c, JacobiScalar s) +inline void MatrixBase<Derived>::applyJacobiOnTheLeft(int p, int q, const JacobiRotation<JacobiScalar>& 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<typename Derived> template<typename JacobiScalar> -inline void MatrixBase<Derived>::applyJacobiOnTheRight(int p, int q, JacobiScalar c, JacobiScalar s) +inline void MatrixBase<Derived>::applyJacobiOnTheRight(int p, int q, const JacobiRotation<JacobiScalar>& 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<typename Scalar> -bool ei_makeJacobi(typename NumTraits<Scalar>::Real x, Scalar y, typename NumTraits<Scalar>::Real z, Scalar *c, Scalar *s) +bool ei_makeJacobi(typename NumTraits<Scalar>::Real x, Scalar y, typename NumTraits<Scalar>::Real z, JacobiRotation<Scalar> *j) { typedef typename NumTraits<Scalar>::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<Scalar>::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<typename Derived> -inline bool MatrixBase<Derived>::makeJacobi(int p, int q, Scalar *c, Scalar *s) const +inline bool MatrixBase<Derived>::makeJacobi(int p, int q, JacobiRotation<Scalar> *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<typename VectorX, typename VectorY, typename JacobiScalar> -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<JacobiScalar>& 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<NumTraits<Scalar>::IsComplex,false> cj; for(int i=0; i<alignedStart; ++i) { Scalar xi = x[i]; Scalar yi = y[i]; - x[i] = c * xi + ei_conj(s) * yi; - y[i] = - s * xi + ei_conj(c) * yi; + x[i] = j.c() * xi + ei_conj(j.s()) * yi; + y[i] = -j.s() * xi + ei_conj(j.c()) * yi; } Scalar* px = x + alignedStart; @@ -182,8 +230,8 @@ void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& { Scalar xi = x[i]; Scalar yi = y[i]; - x[i] = c * xi + ei_conj(s) * yi; - y[i] = -s * xi + ei_conj(c) * yi; + x[i] = j.c() * xi + ei_conj(j.s()) * yi; + y[i] = -j.s() * xi + ei_conj(j.c()) * yi; } } else @@ -192,8 +240,8 @@ void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& { Scalar xi = *x; Scalar yi = *y; - *x = c * xi + ei_conj(s) * yi; - *y = -s * xi + ei_conj(c) * yi; + *x = j.c() * xi + ei_conj(j.s()) * yi; + *y = -j.s() * xi + ei_conj(j.c()) * yi; x += incrx; y += incry; } diff --git a/Eigen/src/SVD/JacobiSVD.h b/Eigen/src/SVD/JacobiSVD.h index e5e7fb6fc..b3d289706 100644 --- a/Eigen/src/SVD/JacobiSVD.h +++ b/Eigen/src/SVD/JacobiSVD.h @@ -54,7 +54,7 @@ template<typename MatrixType, unsigned int Options> class JacobiSVD MaxDiagSizeAtCompileTime = EIGEN_ENUM_MIN(MaxRowsAtCompileTime,MaxColsAtCompileTime), MatrixOptions = MatrixType::Options }; - + typedef Matrix<Scalar, Dynamic, Dynamic, MatrixOptions> DummyMatrixType; typedef typename ei_meta_if<ComputeU, Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime, @@ -73,13 +73,13 @@ template<typename MatrixType, unsigned int Options> 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<typename MatrixType, unsigned int Options> class JacobiSVD MatrixVType m_matrixV; SingularValuesType m_singularValues; bool m_isInitialized; - + template<typename _MatrixType, unsigned int _Options, bool _IsComplex> friend struct ei_svd_precondition_2x2_block_to_be_real; }; @@ -120,11 +120,12 @@ struct ei_svd_precondition_2x2_block_to_be_real<MatrixType, Options, true> typedef JacobiSVD<MatrixType, Options> 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<MatrixType, Options>& svd, int p, int q) { - Scalar c, s, z; + Scalar z; + JacobiRotation<Scalar> 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<MatrixType, Options, true> } 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<MatrixType, Options, true> if(ComputeU) svd.m_matrixU.col(q) *= ei_conj(z); } } - } + } }; template<typename MatrixType, typename RealScalar> 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<RealScalar> *j_left, + JacobiRotation<RealScalar> *j_right) { Matrix<RealScalar,2,2> 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<RealScalar> 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<typename MatrixType, unsigned int Options> @@ -208,18 +205,18 @@ sweep_again: { ei_svd_precondition_2x2_block_to_be_real<MatrixType, Options>::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<RealScalar> 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; } |