00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef EIGEN_SELFADJOINTEIGENSOLVER_H
00027 #define EIGEN_SELFADJOINTEIGENSOLVER_H
00028
00029 #include "./Tridiagonalization.h"
00030
00031 namespace Eigen {
00032
00033 template<typename _MatrixType>
00034 class GeneralizedSelfAdjointEigenSolver;
00035
00036 namespace internal {
00037 template<typename SolverType,int Size,bool IsComplex> struct direct_selfadjoint_eigenvalues;
00038 }
00039
00083 template<typename _MatrixType> class SelfAdjointEigenSolver
00084 {
00085 public:
00086
00087 typedef _MatrixType MatrixType;
00088 enum {
00089 Size = MatrixType::RowsAtCompileTime,
00090 ColsAtCompileTime = MatrixType::ColsAtCompileTime,
00091 Options = MatrixType::Options,
00092 MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
00093 };
00094
00096 typedef typename MatrixType::Scalar Scalar;
00097 typedef typename MatrixType::Index Index;
00098
00105 typedef typename NumTraits<Scalar>::Real RealScalar;
00106
00107 friend struct internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver,Size,NumTraits<Scalar>::IsComplex>;
00108
00114 typedef typename internal::plain_col_type<MatrixType, RealScalar>::type RealVectorType;
00115 typedef Tridiagonalization<MatrixType> TridiagonalizationType;
00116
00127 SelfAdjointEigenSolver()
00128 : m_eivec(),
00129 m_eivalues(),
00130 m_subdiag(),
00131 m_isInitialized(false)
00132 { }
00133
00146 SelfAdjointEigenSolver(Index size)
00147 : m_eivec(size, size),
00148 m_eivalues(size),
00149 m_subdiag(size > 1 ? size - 1 : 1),
00150 m_isInitialized(false)
00151 {}
00152
00168 SelfAdjointEigenSolver(const MatrixType& matrix, int options = ComputeEigenvectors)
00169 : m_eivec(matrix.rows(), matrix.cols()),
00170 m_eivalues(matrix.cols()),
00171 m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1),
00172 m_isInitialized(false)
00173 {
00174 compute(matrix, options);
00175 }
00176
00207 SelfAdjointEigenSolver& compute(const MatrixType& matrix, int options = ComputeEigenvectors);
00208
00223 SelfAdjointEigenSolver& computeDirect(const MatrixType& matrix, int options = ComputeEigenvectors);
00224
00243 const MatrixType& eigenvectors() const
00244 {
00245 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
00246 eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
00247 return m_eivec;
00248 }
00249
00265 const RealVectorType& eigenvalues() const
00266 {
00267 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
00268 return m_eivalues;
00269 }
00270
00289 MatrixType operatorSqrt() const
00290 {
00291 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
00292 eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
00293 return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint();
00294 }
00295
00314 MatrixType operatorInverseSqrt() const
00315 {
00316 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
00317 eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
00318 return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint();
00319 }
00320
00325 ComputationInfo info() const
00326 {
00327 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
00328 return m_info;
00329 }
00330
00336 static const int m_maxIterations = 30;
00337
00338 #ifdef EIGEN2_SUPPORT
00339 SelfAdjointEigenSolver(const MatrixType& matrix, bool computeEigenvectors)
00340 : m_eivec(matrix.rows(), matrix.cols()),
00341 m_eivalues(matrix.cols()),
00342 m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1),
00343 m_isInitialized(false)
00344 {
00345 compute(matrix, computeEigenvectors);
00346 }
00347
00348 SelfAdjointEigenSolver(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true)
00349 : m_eivec(matA.cols(), matA.cols()),
00350 m_eivalues(matA.cols()),
00351 m_subdiag(matA.cols() > 1 ? matA.cols() - 1 : 1),
00352 m_isInitialized(false)
00353 {
00354 static_cast<GeneralizedSelfAdjointEigenSolver<MatrixType>*>(this)->compute(matA, matB, computeEigenvectors ? ComputeEigenvectors : EigenvaluesOnly);
00355 }
00356
00357 void compute(const MatrixType& matrix, bool computeEigenvectors)
00358 {
00359 compute(matrix, computeEigenvectors ? ComputeEigenvectors : EigenvaluesOnly);
00360 }
00361
00362 void compute(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true)
00363 {
00364 compute(matA, matB, computeEigenvectors ? ComputeEigenvectors : EigenvaluesOnly);
00365 }
00366 #endif // EIGEN2_SUPPORT
00367
00368 protected:
00369 MatrixType m_eivec;
00370 RealVectorType m_eivalues;
00371 typename TridiagonalizationType::SubDiagonalType m_subdiag;
00372 ComputationInfo m_info;
00373 bool m_isInitialized;
00374 bool m_eigenvectorsOk;
00375 };
00376
00393 namespace internal {
00394 template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
00395 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n);
00396 }
00397
00398 template<typename MatrixType>
00399 SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
00400 ::compute(const MatrixType& matrix, int options)
00401 {
00402 eigen_assert(matrix.cols() == matrix.rows());
00403 eigen_assert((options&~(EigVecMask|GenEigMask))==0
00404 && (options&EigVecMask)!=EigVecMask
00405 && "invalid option parameter");
00406 bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
00407 Index n = matrix.cols();
00408 m_eivalues.resize(n,1);
00409
00410 if(n==1)
00411 {
00412 m_eivalues.coeffRef(0,0) = internal::real(matrix.coeff(0,0));
00413 if(computeEigenvectors)
00414 m_eivec.setOnes(n,n);
00415 m_info = Success;
00416 m_isInitialized = true;
00417 m_eigenvectorsOk = computeEigenvectors;
00418 return *this;
00419 }
00420
00421
00422 RealVectorType& diag = m_eivalues;
00423 MatrixType& mat = m_eivec;
00424
00425
00426 RealScalar scale = matrix.cwiseAbs().maxCoeff();
00427 if(scale==RealScalar(0)) scale = RealScalar(1);
00428 mat = matrix / scale;
00429 m_subdiag.resize(n-1);
00430 internal::tridiagonalization_inplace(mat, diag, m_subdiag, computeEigenvectors);
00431
00432 Index end = n-1;
00433 Index start = 0;
00434 Index iter = 0;
00435
00436 while (end>0)
00437 {
00438 for (Index i = start; i<end; ++i)
00439 if (internal::isMuchSmallerThan(internal::abs(m_subdiag[i]),(internal::abs(diag[i])+internal::abs(diag[i+1]))))
00440 m_subdiag[i] = 0;
00441
00442
00443 while (end>0 && m_subdiag[end-1]==0)
00444 {
00445 end--;
00446 }
00447 if (end<=0)
00448 break;
00449
00450
00451 iter++;
00452 if(iter > m_maxIterations * n) break;
00453
00454 start = end - 1;
00455 while (start>0 && m_subdiag[start-1]!=0)
00456 start--;
00457
00458 internal::tridiagonal_qr_step<MatrixType::Flags&RowMajorBit ? RowMajor : ColMajor>(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n);
00459 }
00460
00461 if (iter <= m_maxIterations * n)
00462 m_info = Success;
00463 else
00464 m_info = NoConvergence;
00465
00466
00467
00468
00469 if (m_info == Success)
00470 {
00471 for (Index i = 0; i < n-1; ++i)
00472 {
00473 Index k;
00474 m_eivalues.segment(i,n-i).minCoeff(&k);
00475 if (k > 0)
00476 {
00477 std::swap(m_eivalues[i], m_eivalues[k+i]);
00478 if(computeEigenvectors)
00479 m_eivec.col(i).swap(m_eivec.col(k+i));
00480 }
00481 }
00482 }
00483
00484
00485 m_eivalues *= scale;
00486
00487 m_isInitialized = true;
00488 m_eigenvectorsOk = computeEigenvectors;
00489 return *this;
00490 }
00491
00492
00493 namespace internal {
00494
00495 template<typename SolverType,int Size,bool IsComplex> struct direct_selfadjoint_eigenvalues
00496 {
00497 static inline void run(SolverType& eig, const typename SolverType::MatrixType& A, int options)
00498 { eig.compute(A,options); }
00499 };
00500
00501 template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3,false>
00502 {
00503 typedef typename SolverType::MatrixType MatrixType;
00504 typedef typename SolverType::RealVectorType VectorType;
00505 typedef typename SolverType::Scalar Scalar;
00506
00507 static inline void computeRoots(const MatrixType& m, VectorType& roots)
00508 {
00509 using std::sqrt;
00510 using std::atan2;
00511 using std::cos;
00512 using std::sin;
00513 const Scalar s_inv3 = Scalar(1.0)/Scalar(3.0);
00514 const Scalar s_sqrt3 = sqrt(Scalar(3.0));
00515
00516
00517
00518
00519 Scalar c0 = m(0,0)*m(1,1)*m(2,2) + Scalar(2)*m(1,0)*m(2,0)*m(2,1) - m(0,0)*m(2,1)*m(2,1) - m(1,1)*m(2,0)*m(2,0) - m(2,2)*m(1,0)*m(1,0);
00520 Scalar c1 = m(0,0)*m(1,1) - m(1,0)*m(1,0) + m(0,0)*m(2,2) - m(2,0)*m(2,0) + m(1,1)*m(2,2) - m(2,1)*m(2,1);
00521 Scalar c2 = m(0,0) + m(1,1) + m(2,2);
00522
00523
00524
00525 Scalar c2_over_3 = c2*s_inv3;
00526 Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
00527 if (a_over_3 > Scalar(0))
00528 a_over_3 = Scalar(0);
00529
00530 Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));
00531
00532 Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
00533 if (q > Scalar(0))
00534 q = Scalar(0);
00535
00536
00537 Scalar rho = sqrt(-a_over_3);
00538 Scalar theta = atan2(sqrt(-q),half_b)*s_inv3;
00539 Scalar cos_theta = cos(theta);
00540 Scalar sin_theta = sin(theta);
00541 roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta;
00542 roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
00543 roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);
00544
00545
00546 if (roots(0) >= roots(1))
00547 std::swap(roots(0),roots(1));
00548 if (roots(1) >= roots(2))
00549 {
00550 std::swap(roots(1),roots(2));
00551 if (roots(0) >= roots(1))
00552 std::swap(roots(0),roots(1));
00553 }
00554 }
00555
00556 static inline void run(SolverType& solver, const MatrixType& mat, int options)
00557 {
00558 using std::sqrt;
00559 eigen_assert(mat.cols() == 3 && mat.cols() == mat.rows());
00560 eigen_assert((options&~(EigVecMask|GenEigMask))==0
00561 && (options&EigVecMask)!=EigVecMask
00562 && "invalid option parameter");
00563 bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
00564
00565 MatrixType& eivecs = solver.m_eivec;
00566 VectorType& eivals = solver.m_eivalues;
00567
00568
00569 Scalar scale = mat.cwiseAbs().maxCoeff();
00570 MatrixType scaledMat = mat / scale;
00571
00572
00573 computeRoots(scaledMat,eivals);
00574
00575
00576 if(computeEigenvectors)
00577 {
00578 Scalar safeNorm2 = Eigen::NumTraits<Scalar>::epsilon();
00579 safeNorm2 *= safeNorm2;
00580 if((eivals(2)-eivals(0))<=Eigen::NumTraits<Scalar>::epsilon())
00581 {
00582 eivecs.setIdentity();
00583 }
00584 else
00585 {
00586 scaledMat = scaledMat.template selfadjointView<Lower>();
00587 MatrixType tmp;
00588 tmp = scaledMat;
00589
00590 Scalar d0 = eivals(2) - eivals(1);
00591 Scalar d1 = eivals(1) - eivals(0);
00592 int k = d0 > d1 ? 2 : 0;
00593 d0 = d0 > d1 ? d1 : d0;
00594
00595 tmp.diagonal().array () -= eivals(k);
00596 VectorType cross;
00597 Scalar n;
00598 n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm();
00599
00600 if(n>safeNorm2)
00601 eivecs.col(k) = cross / sqrt(n);
00602 else
00603 {
00604 n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm();
00605
00606 if(n>safeNorm2)
00607 eivecs.col(k) = cross / sqrt(n);
00608 else
00609 {
00610 n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm();
00611
00612 if(n>safeNorm2)
00613 eivecs.col(k) = cross / sqrt(n);
00614 else
00615 {
00616
00617
00618
00619 eivals *= scale;
00620
00621 solver.m_info = NumericalIssue;
00622 solver.m_isInitialized = true;
00623 solver.m_eigenvectorsOk = computeEigenvectors;
00624 return;
00625 }
00626 }
00627 }
00628
00629 tmp = scaledMat;
00630 tmp.diagonal().array() -= eivals(1);
00631
00632 if(d0<=Eigen::NumTraits<Scalar>::epsilon())
00633 eivecs.col(1) = eivecs.col(k).unitOrthogonal();
00634 else
00635 {
00636 n = (cross = eivecs.col(k).cross(tmp.row(0).normalized())).squaredNorm();
00637 if(n>safeNorm2)
00638 eivecs.col(1) = cross / sqrt(n);
00639 else
00640 {
00641 n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm();
00642 if(n>safeNorm2)
00643 eivecs.col(1) = cross / sqrt(n);
00644 else
00645 {
00646 n = (cross = eivecs.col(k).cross(tmp.row(2))).squaredNorm();
00647 if(n>safeNorm2)
00648 eivecs.col(1) = cross / sqrt(n);
00649 else
00650 {
00651
00652
00653 eivecs.col(1) = eivecs.col(k).unitOrthogonal();
00654 }
00655 }
00656 }
00657
00658
00659 Scalar d = eivecs.col(1).dot(eivecs.col(k));
00660 eivecs.col(1) = (eivecs.col(1) - d * eivecs.col(k)).normalized();
00661 }
00662
00663 eivecs.col(k==2 ? 0 : 2) = eivecs.col(k).cross(eivecs.col(1)).normalized();
00664 }
00665 }
00666
00667 eivals *= scale;
00668
00669 solver.m_info = Success;
00670 solver.m_isInitialized = true;
00671 solver.m_eigenvectorsOk = computeEigenvectors;
00672 }
00673 };
00674
00675
00676 template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,2,false>
00677 {
00678 typedef typename SolverType::MatrixType MatrixType;
00679 typedef typename SolverType::RealVectorType VectorType;
00680 typedef typename SolverType::Scalar Scalar;
00681
00682 static inline void computeRoots(const MatrixType& m, VectorType& roots)
00683 {
00684 using std::sqrt;
00685 const Scalar t0 = Scalar(0.5) * sqrt( abs2(m(0,0)-m(1,1)) + Scalar(4)*m(1,0)*m(1,0));
00686 const Scalar t1 = Scalar(0.5) * (m(0,0) + m(1,1));
00687 roots(0) = t1 - t0;
00688 roots(1) = t1 + t0;
00689 }
00690
00691 static inline void run(SolverType& solver, const MatrixType& mat, int options)
00692 {
00693 eigen_assert(mat.cols() == 2 && mat.cols() == mat.rows());
00694 eigen_assert((options&~(EigVecMask|GenEigMask))==0
00695 && (options&EigVecMask)!=EigVecMask
00696 && "invalid option parameter");
00697 bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
00698
00699 MatrixType& eivecs = solver.m_eivec;
00700 VectorType& eivals = solver.m_eivalues;
00701
00702
00703 Scalar scale = mat.cwiseAbs().maxCoeff();
00704 scale = (std::max)(scale,Scalar(1));
00705 MatrixType scaledMat = mat / scale;
00706
00707
00708 computeRoots(scaledMat,eivals);
00709
00710
00711 if(computeEigenvectors)
00712 {
00713 scaledMat.diagonal().array () -= eivals(1);
00714 Scalar a2 = abs2(scaledMat(0,0));
00715 Scalar c2 = abs2(scaledMat(1,1));
00716 Scalar b2 = abs2(scaledMat(1,0));
00717 if(a2>c2)
00718 {
00719 eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0);
00720 eivecs.col(1) /= sqrt(a2+b2);
00721 }
00722 else
00723 {
00724 eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0);
00725 eivecs.col(1) /= sqrt(c2+b2);
00726 }
00727
00728 eivecs.col(0) << eivecs.col(1).unitOrthogonal();
00729 }
00730
00731
00732 eivals *= scale;
00733
00734 solver.m_info = Success;
00735 solver.m_isInitialized = true;
00736 solver.m_eigenvectorsOk = computeEigenvectors;
00737 }
00738 };
00739
00740 }
00741
00742 template<typename MatrixType>
00743 SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
00744 ::computeDirect(const MatrixType& matrix, int options)
00745 {
00746 internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver,Size,NumTraits<Scalar>::IsComplex>::run(*this,matrix,options);
00747 return *this;
00748 }
00749
00750 namespace internal {
00751 template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
00752 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n)
00753 {
00754
00755
00756
00757
00758
00759
00760 RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5);
00761 RealScalar e2 = abs2(subdiag[end-1]);
00762 RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2));
00763 RealScalar x = diag[start] - mu;
00764 RealScalar z = subdiag[start];
00765 for (Index k = start; k < end; ++k)
00766 {
00767 JacobiRotation<RealScalar> rot;
00768 rot.makeGivens(x, z);
00769
00770
00771 RealScalar sdk = rot.s() * diag[k] + rot.c() * subdiag[k];
00772 RealScalar dkp1 = rot.s() * subdiag[k] + rot.c() * diag[k+1];
00773
00774 diag[k] = rot.c() * (rot.c() * diag[k] - rot.s() * subdiag[k]) - rot.s() * (rot.c() * subdiag[k] - rot.s() * diag[k+1]);
00775 diag[k+1] = rot.s() * sdk + rot.c() * dkp1;
00776 subdiag[k] = rot.c() * sdk - rot.s() * dkp1;
00777
00778
00779 if (k > start)
00780 subdiag[k - 1] = rot.c() * subdiag[k-1] - rot.s() * z;
00781
00782 x = subdiag[k];
00783
00784 if (k < end - 1)
00785 {
00786 z = -rot.s() * subdiag[k+1];
00787 subdiag[k + 1] = rot.c() * subdiag[k+1];
00788 }
00789
00790
00791 if (matrixQ)
00792 {
00793
00794 Map<Matrix<Scalar,Dynamic,Dynamic,StorageOrder> > q(matrixQ,n,n);
00795 q.applyOnTheRight(k,k+1,rot);
00796 }
00797 }
00798 }
00799
00800 }
00801
00802 }
00803
00804 #endif // EIGEN_SELFADJOINTEIGENSOLVER_H