diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h index 688b7c4d0..52d5f680c 100644 --- a/Eigen/src/Core/MatrixBase.h +++ b/Eigen/src/Core/MatrixBase.h @@ -782,8 +782,9 @@ template class MatrixBase void applyJacobiOnTheLeft(int p, int q, Scalar c, Scalar s); void applyJacobiOnTheRight(int p, int q, Scalar c, Scalar s); - bool makeJacobi(int p, int q, Scalar max_coeff, Scalar *c, Scalar *s); - bool makeJacobiForAtA(int p, int q, Scalar max_coeff, Scalar *c, Scalar *s); + bool makeJacobi(int p, int q, Scalar *c, Scalar *s) const; + bool makeJacobiForAtA(int p, int q, Scalar *c, Scalar *s) const; + bool makeJacobiForAAt(int p, int q, Scalar *c, Scalar *s) const; #ifdef EIGEN_MATRIXBASE_PLUGIN #include EIGEN_MATRIXBASE_PLUGIN diff --git a/Eigen/src/Jacobi/CMakeLists.txt b/Eigen/src/Jacobi/CMakeLists.txt new file mode 100644 index 000000000..490dac626 --- /dev/null +++ b/Eigen/src/Jacobi/CMakeLists.txt @@ -0,0 +1,6 @@ +FILE(GLOB Eigen_Jacobi_SRCS "*.h") + +INSTALL(FILES + ${Eigen_Jacobi_SRCS} + DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Jacobi COMPONENT Devel + ) diff --git a/Eigen/src/Jacobi/Jacobi.h b/Eigen/src/Jacobi/Jacobi.h index 993a723ab..5866ac44f 100644 --- a/Eigen/src/Jacobi/Jacobi.h +++ b/Eigen/src/Jacobi/Jacobi.h @@ -48,13 +48,13 @@ void MatrixBase::applyJacobiOnTheRight(int p, int q, Scalar c, Scalar s } template -bool ei_makeJacobi(Scalar x, Scalar y, Scalar z, Scalar max_coeff, Scalar *c, Scalar *s) +bool ei_makeJacobi(Scalar x, Scalar y, Scalar z, Scalar *c, Scalar *s) { - if(ei_abs(y) < max_coeff * 0.5 * machine_epsilon()) + if(ei_abs(y) < ei_abs(z-x) * 0.5 * machine_epsilon()) { *c = Scalar(1); *s = Scalar(0); - return true; + return false; } else { @@ -67,23 +67,31 @@ bool ei_makeJacobi(Scalar x, Scalar y, Scalar z, Scalar max_coeff, Scalar *c, Sc t = Scalar(1) / (tau - w); *c = Scalar(1) / ei_sqrt(1 + ei_abs2(t)); *s = *c * t; - return false; + return true; } } template -inline bool MatrixBase::makeJacobi(int p, int q, Scalar max_coeff, Scalar *c, Scalar *s) +inline bool MatrixBase::makeJacobi(int p, int q, Scalar *c, Scalar *s) const { - return ei_makeJacobi(coeff(p,p), coeff(p,q), coeff(q,q), max_coeff, c, s); + return ei_makeJacobi(coeff(p,p), coeff(p,q), coeff(q,q), c, s); } template -inline bool MatrixBase::makeJacobiForAtA(int p, int q, Scalar max_coeff, Scalar *c, Scalar *s) +inline bool MatrixBase::makeJacobiForAtA(int p, int q, Scalar *c, Scalar *s) const { - return ei_makeJacobi(col(p).squaredNorm(), - col(p).dot(col(q)), - col(q).squaredNorm(), - max_coeff, + return ei_makeJacobi(ei_abs2(coeff(p,p)) + ei_abs2(coeff(q,p)), + ei_conj(coeff(p,p))*coeff(p,q) + ei_conj(coeff(q,p))*coeff(q,q), + ei_abs2(coeff(p,q)) + ei_abs2(coeff(q,q)), + c,s); +} + +template +inline bool MatrixBase::makeJacobiForAAt(int p, int q, Scalar *c, Scalar *s) const +{ + return ei_makeJacobi(ei_abs2(coeff(p,p)) + ei_abs2(coeff(p,q)), + ei_conj(coeff(q,p))*coeff(p,p) + ei_conj(coeff(q,q))*coeff(p,q), + ei_abs2(coeff(q,p)) + ei_abs2(coeff(q,q)), c,s); } diff --git a/Eigen/src/SVD/JacobiSquareSVD.h b/Eigen/src/SVD/JacobiSquareSVD.h index ad55735fc..18416e312 100644 --- a/Eigen/src/SVD/JacobiSquareSVD.h +++ b/Eigen/src/SVD/JacobiSquareSVD.h @@ -102,69 +102,65 @@ void JacobiSquareSVD::compute(const MatrixType& if(ComputeU) m_matrixU = MatrixUType::Identity(size,size); if(ComputeV) m_matrixV = MatrixUType::Identity(size,size); m_singularValues.resize(size); - RealScalar max_coeff = work_matrix.cwise().abs().maxCoeff(); - for(int k = 1; k < 40; ++k) { + while(true) + { bool finished = true; for(int p = 1; p < size; ++p) { for(int q = 0; q < p; ++q) { Scalar c, s; - finished &= work_matrix.makeJacobiForAtA(p,q,max_coeff,&c,&s); - work_matrix.applyJacobiOnTheRight(p,q,c,s); - if(ComputeV) m_matrixV.applyJacobiOnTheRight(p,q,c,s); + if(work_matrix.makeJacobiForAtA(p,q,&c,&s)) + { + work_matrix.applyJacobiOnTheRight(p,q,c,s); + if(ComputeV) m_matrixV.applyJacobiOnTheRight(p,q,c,s); + } + if(work_matrix.makeJacobiForAAt(p,q,&c,&s)) + { + work_matrix.applyJacobiOnTheLeft(p,q,c,s); + if(ComputeU) m_matrixU.applyJacobiOnTheRight(p,q,c,s); + if(std::max(ei_abs(work_matrix.coeff(p,q)), ei_abs(work_matrix.coeff(q,p))) > std::max(ei_abs(work_matrix.coeff(q,q)), ei_abs(work_matrix.coeff(p,p)) )) + { + work_matrix.row(p).swap(work_matrix.row(q)); + if(ComputeU) m_matrixU.col(p).swap(m_matrixU.col(q)); + } + } + } + } + RealScalar biggest = work_matrix.diagonal().cwise().abs().maxCoeff(); + for(int p = 0; p < size; ++p) + { + for(int q = 0; q < size; ++q) + { + if(p!=q && ei_abs(work_matrix.coeff(p,q)) > biggest * machine_epsilon()) finished = false; } } if(finished) break; } - + + m_singularValues = work_matrix.diagonal().cwise().abs(); + RealScalar biggestSingularValue = m_singularValues.maxCoeff(); + for(int i = 0; i < size; ++i) { - m_singularValues.coeffRef(i) = work_matrix.col(i).norm(); + RealScalar a = ei_abs(work_matrix.coeff(i,i)); + m_singularValues.coeffRef(i) = a; + if(ComputeU && !ei_isMuchSmallerThan(a, biggestSingularValue)) m_matrixU.col(i) *= work_matrix.coeff(i,i)/a; } - int first_zero = size; - RealScalar biggest = m_singularValues.maxCoeff(); for(int i = 0; i < size; i++) { int pos; - RealScalar biggest_remaining = m_singularValues.end(size-i).maxCoeff(&pos); - if(first_zero == size && ei_isMuchSmallerThan(biggest_remaining, biggest)) first_zero = pos + i; + m_singularValues.end(size-i).maxCoeff(&pos); if(pos) { pos += i; std::swap(m_singularValues.coeffRef(i), m_singularValues.coeffRef(pos)); - if(ComputeU) work_matrix.col(pos).swap(work_matrix.col(i)); + if(ComputeU) m_matrixU.col(pos).swap(m_matrixU.col(i)); if(ComputeV) m_matrixV.col(pos).swap(m_matrixV.col(i)); } } - - if(ComputeU) - { - for(int i = 0; i < first_zero; ++i) - { - m_matrixU.col(i) = work_matrix.col(i) / m_singularValues.coeff(i); - } - if(first_zero < size) - { - for(int i = first_zero; i < size; ++i) - { - for(int j = 0; j < size; ++j) - { - m_matrixU.col(i).setZero(); - m_matrixU.coeffRef(j,i) = Scalar(1); - for(int k = 0; k < first_zero; ++k) - m_matrixU.col(i) -= m_matrixU.col(i).dot(m_matrixU.col(k)) * m_matrixU.col(k); - RealScalar n = m_matrixU.col(i).norm(); - if(!ei_isMuchSmallerThan(n, biggest)) - { - m_matrixU.col(i) /= n; - break; - } - } - } - } - } + m_isInitialized = true; } #endif // EIGEN_JACOBISQUARESVD_H