![]() |
Eigen
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr> 00005 // Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk> 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_SELFADJOINTEIGENSOLVER_H 00012 #define EIGEN_SELFADJOINTEIGENSOLVER_H 00013 00014 #include "./Tridiagonalization.h" 00015 00016 namespace Eigen { 00017 00018 template<typename _MatrixType> 00019 class GeneralizedSelfAdjointEigenSolver; 00020 00021 namespace internal { 00022 template<typename SolverType,int Size,bool IsComplex> struct direct_selfadjoint_eigenvalues; 00023 template<typename MatrixType, typename DiagType, typename SubDiagType> 00024 ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag, const Index maxIterations, bool computeEigenvectors, MatrixType& eivec); 00025 } 00026 00070 template<typename _MatrixType> class SelfAdjointEigenSolver 00071 { 00072 public: 00073 00074 typedef _MatrixType MatrixType; 00075 enum { 00076 Size = MatrixType::RowsAtCompileTime, 00077 ColsAtCompileTime = MatrixType::ColsAtCompileTime, 00078 Options = MatrixType::Options, 00079 MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime 00080 }; 00081 00083 typedef typename MatrixType::Scalar Scalar; 00084 typedef Eigen::Index Index; 00085 00086 typedef Matrix<Scalar,Size,Size,ColMajor,MaxColsAtCompileTime,MaxColsAtCompileTime> EigenvectorsType; 00087 00094 typedef typename NumTraits<Scalar>::Real RealScalar; 00095 00096 friend struct internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver,Size,NumTraits<Scalar>::IsComplex>; 00097 00103 typedef typename internal::plain_col_type<MatrixType, RealScalar>::type RealVectorType; 00104 typedef Tridiagonalization<MatrixType> TridiagonalizationType; 00105 typedef typename TridiagonalizationType::SubDiagonalType SubDiagonalType; 00106 00117 EIGEN_DEVICE_FUNC 00118 SelfAdjointEigenSolver() 00119 : m_eivec(), 00120 m_eivalues(), 00121 m_subdiag(), 00122 m_isInitialized(false) 00123 { } 00124 00137 EIGEN_DEVICE_FUNC 00138 explicit SelfAdjointEigenSolver(Index size) 00139 : m_eivec(size, size), 00140 m_eivalues(size), 00141 m_subdiag(size > 1 ? size - 1 : 1), 00142 m_isInitialized(false) 00143 {} 00144 00160 template<typename InputType> 00161 EIGEN_DEVICE_FUNC 00162 explicit SelfAdjointEigenSolver(const EigenBase<InputType>& matrix, int options = ComputeEigenvectors) 00163 : m_eivec(matrix.rows(), matrix.cols()), 00164 m_eivalues(matrix.cols()), 00165 m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1), 00166 m_isInitialized(false) 00167 { 00168 compute(matrix.derived(), options); 00169 } 00170 00201 template<typename InputType> 00202 EIGEN_DEVICE_FUNC 00203 SelfAdjointEigenSolver& compute(const EigenBase<InputType>& matrix, int options = ComputeEigenvectors); 00204 00223 EIGEN_DEVICE_FUNC 00224 SelfAdjointEigenSolver& computeDirect(const MatrixType& matrix, int options = ComputeEigenvectors); 00225 00238 SelfAdjointEigenSolver& computeFromTridiagonal(const RealVectorType& diag, const SubDiagonalType& subdiag , int options=ComputeEigenvectors); 00239 00258 EIGEN_DEVICE_FUNC 00259 const EigenvectorsType& eigenvectors() const 00260 { 00261 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); 00262 eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); 00263 return m_eivec; 00264 } 00265 00281 EIGEN_DEVICE_FUNC 00282 const RealVectorType& eigenvalues() const 00283 { 00284 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); 00285 return m_eivalues; 00286 } 00287 00305 EIGEN_DEVICE_FUNC 00306 MatrixType operatorSqrt() const 00307 { 00308 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); 00309 eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); 00310 return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint(); 00311 } 00312 00330 EIGEN_DEVICE_FUNC 00331 MatrixType operatorInverseSqrt() const 00332 { 00333 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); 00334 eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); 00335 return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint(); 00336 } 00337 00342 EIGEN_DEVICE_FUNC 00343 ComputationInfo info() const 00344 { 00345 eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); 00346 return m_info; 00347 } 00348 00354 static const int m_maxIterations = 30; 00355 00356 protected: 00357 static void check_template_parameters() 00358 { 00359 EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); 00360 } 00361 00362 EigenvectorsType m_eivec; 00363 RealVectorType m_eivalues; 00364 typename TridiagonalizationType::SubDiagonalType m_subdiag; 00365 ComputationInfo m_info; 00366 bool m_isInitialized; 00367 bool m_eigenvectorsOk; 00368 }; 00369 00370 namespace internal { 00391 template<int StorageOrder,typename RealScalar, typename Scalar, typename Index> 00392 EIGEN_DEVICE_FUNC 00393 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n); 00394 } 00395 00396 template<typename MatrixType> 00397 template<typename InputType> 00398 EIGEN_DEVICE_FUNC 00399 SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType> 00400 ::compute(const EigenBase<InputType>& a_matrix, int options) 00401 { 00402 check_template_parameters(); 00403 00404 const InputType &matrix(a_matrix.derived()); 00405 00406 using std::abs; 00407 eigen_assert(matrix.cols() == matrix.rows()); 00408 eigen_assert((options&~(EigVecMask|GenEigMask))==0 00409 && (options&EigVecMask)!=EigVecMask 00410 && "invalid option parameter"); 00411 bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; 00412 Index n = matrix.cols(); 00413 m_eivalues.resize(n,1); 00414 00415 if(n==1) 00416 { 00417 m_eivec = matrix; 00418 m_eivalues.coeffRef(0,0) = numext::real(m_eivec.coeff(0,0)); 00419 if(computeEigenvectors) 00420 m_eivec.setOnes(n,n); 00421 m_info = Success; 00422 m_isInitialized = true; 00423 m_eigenvectorsOk = computeEigenvectors; 00424 return *this; 00425 } 00426 00427 // declare some aliases 00428 RealVectorType& diag = m_eivalues; 00429 EigenvectorsType& mat = m_eivec; 00430 00431 // map the matrix coefficients to [-1:1] to avoid over- and underflow. 00432 mat = matrix.template triangularView<Lower>(); 00433 RealScalar scale = mat.cwiseAbs().maxCoeff(); 00434 if(scale==RealScalar(0)) scale = RealScalar(1); 00435 mat.template triangularView<Lower>() /= scale; 00436 m_subdiag.resize(n-1); 00437 internal::tridiagonalization_inplace(mat, diag, m_subdiag, computeEigenvectors); 00438 00439 m_info = internal::computeFromTridiagonal_impl(diag, m_subdiag, m_maxIterations, computeEigenvectors, m_eivec); 00440 00441 // scale back the eigen values 00442 m_eivalues *= scale; 00443 00444 m_isInitialized = true; 00445 m_eigenvectorsOk = computeEigenvectors; 00446 return *this; 00447 } 00448 00449 template<typename MatrixType> 00450 SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType> 00451 ::computeFromTridiagonal(const RealVectorType& diag, const SubDiagonalType& subdiag , int options) 00452 { 00453 //TODO : Add an option to scale the values beforehand 00454 bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; 00455 00456 m_eivalues = diag; 00457 m_subdiag = subdiag; 00458 if (computeEigenvectors) 00459 { 00460 m_eivec.setIdentity(diag.size(), diag.size()); 00461 } 00462 m_info = internal::computeFromTridiagonal_impl(m_eivalues, m_subdiag, m_maxIterations, computeEigenvectors, m_eivec); 00463 00464 m_isInitialized = true; 00465 m_eigenvectorsOk = computeEigenvectors; 00466 return *this; 00467 } 00468 00469 namespace internal { 00481 template<typename MatrixType, typename DiagType, typename SubDiagType> 00482 ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag, const Index maxIterations, bool computeEigenvectors, MatrixType& eivec) 00483 { 00484 using std::abs; 00485 00486 ComputationInfo info; 00487 typedef typename MatrixType::Scalar Scalar; 00488 00489 Index n = diag.size(); 00490 Index end = n-1; 00491 Index start = 0; 00492 Index iter = 0; // total number of iterations 00493 00494 typedef typename DiagType::RealScalar RealScalar; 00495 const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)(); 00496 const RealScalar precision = RealScalar(2)*NumTraits<RealScalar>::epsilon(); 00497 00498 while (end>0) 00499 { 00500 for (Index i = start; i<end; ++i) 00501 if (internal::isMuchSmallerThan(abs(subdiag[i]),(abs(diag[i])+abs(diag[i+1])),precision) || abs(subdiag[i]) <= considerAsZero) 00502 subdiag[i] = 0; 00503 00504 // find the largest unreduced block 00505 while (end>0 && subdiag[end-1]==RealScalar(0)) 00506 { 00507 end--; 00508 } 00509 if (end<=0) 00510 break; 00511 00512 // if we spent too many iterations, we give up 00513 iter++; 00514 if(iter > maxIterations * n) break; 00515 00516 start = end - 1; 00517 while (start>0 && subdiag[start-1]!=0) 00518 start--; 00519 00520 internal::tridiagonal_qr_step<MatrixType::Flags&RowMajorBit ? RowMajor : ColMajor>(diag.data(), subdiag.data(), start, end, computeEigenvectors ? eivec.data() : (Scalar*)0, n); 00521 } 00522 if (iter <= maxIterations * n) 00523 info = Success; 00524 else 00525 info = NoConvergence; 00526 00527 // Sort eigenvalues and corresponding vectors. 00528 // TODO make the sort optional ? 00529 // TODO use a better sort algorithm !! 00530 if (info == Success) 00531 { 00532 for (Index i = 0; i < n-1; ++i) 00533 { 00534 Index k; 00535 diag.segment(i,n-i).minCoeff(&k); 00536 if (k > 0) 00537 { 00538 std::swap(diag[i], diag[k+i]); 00539 if(computeEigenvectors) 00540 eivec.col(i).swap(eivec.col(k+i)); 00541 } 00542 } 00543 } 00544 return info; 00545 } 00546 00547 template<typename SolverType,int Size,bool IsComplex> struct direct_selfadjoint_eigenvalues 00548 { 00549 EIGEN_DEVICE_FUNC 00550 static inline void run(SolverType& eig, const typename SolverType::MatrixType& A, int options) 00551 { eig.compute(A,options); } 00552 }; 00553 00554 template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3,false> 00555 { 00556 typedef typename SolverType::MatrixType MatrixType; 00557 typedef typename SolverType::RealVectorType VectorType; 00558 typedef typename SolverType::Scalar Scalar; 00559 typedef typename SolverType::EigenvectorsType EigenvectorsType; 00560 00561 00566 EIGEN_DEVICE_FUNC 00567 static inline void computeRoots(const MatrixType& m, VectorType& roots) 00568 { 00569 EIGEN_USING_STD_MATH(sqrt) 00570 EIGEN_USING_STD_MATH(atan2) 00571 EIGEN_USING_STD_MATH(cos) 00572 EIGEN_USING_STD_MATH(sin) 00573 const Scalar s_inv3 = Scalar(1)/Scalar(3); 00574 const Scalar s_sqrt3 = sqrt(Scalar(3)); 00575 00576 // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The 00577 // eigenvalues are the roots to this equation, all guaranteed to be 00578 // real-valued, because the matrix is symmetric. 00579 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); 00580 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); 00581 Scalar c2 = m(0,0) + m(1,1) + m(2,2); 00582 00583 // Construct the parameters used in classifying the roots of the equation 00584 // and in solving the equation for the roots in closed form. 00585 Scalar c2_over_3 = c2*s_inv3; 00586 Scalar a_over_3 = (c2*c2_over_3 - c1)*s_inv3; 00587 a_over_3 = numext::maxi(a_over_3, Scalar(0)); 00588 00589 Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1)); 00590 00591 Scalar q = a_over_3*a_over_3*a_over_3 - half_b*half_b; 00592 q = numext::maxi(q, Scalar(0)); 00593 00594 // Compute the eigenvalues by solving for the roots of the polynomial. 00595 Scalar rho = sqrt(a_over_3); 00596 Scalar theta = atan2(sqrt(q),half_b)*s_inv3; // since sqrt(q) > 0, atan2 is in [0, pi] and theta is in [0, pi/3] 00597 Scalar cos_theta = cos(theta); 00598 Scalar sin_theta = sin(theta); 00599 // roots are already sorted, since cos is monotonically decreasing on [0, pi] 00600 roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); // == 2*rho*cos(theta+2pi/3) 00601 roots(1) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); // == 2*rho*cos(theta+ pi/3) 00602 roots(2) = c2_over_3 + Scalar(2)*rho*cos_theta; 00603 } 00604 00605 EIGEN_DEVICE_FUNC 00606 static inline bool extract_kernel(MatrixType& mat, Ref<VectorType> res, Ref<VectorType> representative) 00607 { 00608 using std::abs; 00609 Index i0; 00610 // Find non-zero column i0 (by construction, there must exist a non zero coefficient on the diagonal): 00611 mat.diagonal().cwiseAbs().maxCoeff(&i0); 00612 // mat.col(i0) is a good candidate for an orthogonal vector to the current eigenvector, 00613 // so let's save it: 00614 representative = mat.col(i0); 00615 Scalar n0, n1; 00616 VectorType c0, c1; 00617 n0 = (c0 = representative.cross(mat.col((i0+1)%3))).squaredNorm(); 00618 n1 = (c1 = representative.cross(mat.col((i0+2)%3))).squaredNorm(); 00619 if(n0>n1) res = c0/std::sqrt(n0); 00620 else res = c1/std::sqrt(n1); 00621 00622 return true; 00623 } 00624 00625 EIGEN_DEVICE_FUNC 00626 static inline void run(SolverType& solver, const MatrixType& mat, int options) 00627 { 00628 eigen_assert(mat.cols() == 3 && mat.cols() == mat.rows()); 00629 eigen_assert((options&~(EigVecMask|GenEigMask))==0 00630 && (options&EigVecMask)!=EigVecMask 00631 && "invalid option parameter"); 00632 bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; 00633 00634 EigenvectorsType& eivecs = solver.m_eivec; 00635 VectorType& eivals = solver.m_eivalues; 00636 00637 // Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow. 00638 Scalar shift = mat.trace() / Scalar(3); 00639 // TODO Avoid this copy. Currently it is necessary to suppress bogus values when determining maxCoeff and for computing the eigenvectors later 00640 MatrixType scaledMat = mat.template selfadjointView<Lower>(); 00641 scaledMat.diagonal().array() -= shift; 00642 Scalar scale = scaledMat.cwiseAbs().maxCoeff(); 00643 if(scale > 0) scaledMat /= scale; // TODO for scale==0 we could save the remaining operations 00644 00645 // compute the eigenvalues 00646 computeRoots(scaledMat,eivals); 00647 00648 // compute the eigenvectors 00649 if(computeEigenvectors) 00650 { 00651 if((eivals(2)-eivals(0))<=Eigen::NumTraits<Scalar>::epsilon()) 00652 { 00653 // All three eigenvalues are numerically the same 00654 eivecs.setIdentity(); 00655 } 00656 else 00657 { 00658 MatrixType tmp; 00659 tmp = scaledMat; 00660 00661 // Compute the eigenvector of the most distinct eigenvalue 00662 Scalar d0 = eivals(2) - eivals(1); 00663 Scalar d1 = eivals(1) - eivals(0); 00664 Index k(0), l(2); 00665 if(d0 > d1) 00666 { 00667 numext::swap(k,l); 00668 d0 = d1; 00669 } 00670 00671 // Compute the eigenvector of index k 00672 { 00673 tmp.diagonal().array () -= eivals(k); 00674 // By construction, 'tmp' is of rank 2, and its kernel corresponds to the respective eigenvector. 00675 extract_kernel(tmp, eivecs.col(k), eivecs.col(l)); 00676 } 00677 00678 // Compute eigenvector of index l 00679 if(d0<=2*Eigen::NumTraits<Scalar>::epsilon()*d1) 00680 { 00681 // If d0 is too small, then the two other eigenvalues are numerically the same, 00682 // and thus we only have to ortho-normalize the near orthogonal vector we saved above. 00683 eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l))*eivecs.col(l); 00684 eivecs.col(l).normalize(); 00685 } 00686 else 00687 { 00688 tmp = scaledMat; 00689 tmp.diagonal().array () -= eivals(l); 00690 00691 VectorType dummy; 00692 extract_kernel(tmp, eivecs.col(l), dummy); 00693 } 00694 00695 // Compute last eigenvector from the other two 00696 eivecs.col(1) = eivecs.col(2).cross(eivecs.col(0)).normalized(); 00697 } 00698 } 00699 00700 // Rescale back to the original size. 00701 eivals *= scale; 00702 eivals.array() += shift; 00703 00704 solver.m_info = Success; 00705 solver.m_isInitialized = true; 00706 solver.m_eigenvectorsOk = computeEigenvectors; 00707 } 00708 }; 00709 00710 // 2x2 direct eigenvalues decomposition, code from Hauke Heibel 00711 template<typename SolverType> 00712 struct direct_selfadjoint_eigenvalues<SolverType,2,false> 00713 { 00714 typedef typename SolverType::MatrixType MatrixType; 00715 typedef typename SolverType::RealVectorType VectorType; 00716 typedef typename SolverType::Scalar Scalar; 00717 typedef typename SolverType::EigenvectorsType EigenvectorsType; 00718 00719 EIGEN_DEVICE_FUNC 00720 static inline void computeRoots(const MatrixType& m, VectorType& roots) 00721 { 00722 using std::sqrt; 00723 const Scalar t0 = Scalar(0.5) * sqrt( numext::abs2(m(0,0)-m(1,1)) + Scalar(4)*numext::abs2(m(1,0))); 00724 const Scalar t1 = Scalar(0.5) * (m(0,0) + m(1,1)); 00725 roots(0) = t1 - t0; 00726 roots(1) = t1 + t0; 00727 } 00728 00729 EIGEN_DEVICE_FUNC 00730 static inline void run(SolverType& solver, const MatrixType& mat, int options) 00731 { 00732 EIGEN_USING_STD_MATH(sqrt); 00733 EIGEN_USING_STD_MATH(abs); 00734 00735 eigen_assert(mat.cols() == 2 && mat.cols() == mat.rows()); 00736 eigen_assert((options&~(EigVecMask|GenEigMask))==0 00737 && (options&EigVecMask)!=EigVecMask 00738 && "invalid option parameter"); 00739 bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; 00740 00741 EigenvectorsType& eivecs = solver.m_eivec; 00742 VectorType& eivals = solver.m_eivalues; 00743 00744 // Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow. 00745 Scalar shift = mat.trace() / Scalar(2); 00746 MatrixType scaledMat = mat; 00747 scaledMat.coeffRef(0,1) = mat.coeff(1,0); 00748 scaledMat.diagonal().array() -= shift; 00749 Scalar scale = scaledMat.cwiseAbs().maxCoeff(); 00750 if(scale > Scalar(0)) 00751 scaledMat /= scale; 00752 00753 // Compute the eigenvalues 00754 computeRoots(scaledMat,eivals); 00755 00756 // compute the eigen vectors 00757 if(computeEigenvectors) 00758 { 00759 if((eivals(1)-eivals(0))<=abs(eivals(1))*Eigen::NumTraits<Scalar>::epsilon()) 00760 { 00761 eivecs.setIdentity(); 00762 } 00763 else 00764 { 00765 scaledMat.diagonal().array () -= eivals(1); 00766 Scalar a2 = numext::abs2(scaledMat(0,0)); 00767 Scalar c2 = numext::abs2(scaledMat(1,1)); 00768 Scalar b2 = numext::abs2(scaledMat(1,0)); 00769 if(a2>c2) 00770 { 00771 eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0); 00772 eivecs.col(1) /= sqrt(a2+b2); 00773 } 00774 else 00775 { 00776 eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0); 00777 eivecs.col(1) /= sqrt(c2+b2); 00778 } 00779 00780 eivecs.col(0) << eivecs.col(1).unitOrthogonal(); 00781 } 00782 } 00783 00784 // Rescale back to the original size. 00785 eivals *= scale; 00786 eivals.array() += shift; 00787 00788 solver.m_info = Success; 00789 solver.m_isInitialized = true; 00790 solver.m_eigenvectorsOk = computeEigenvectors; 00791 } 00792 }; 00793 00794 } 00795 00796 template<typename MatrixType> 00797 EIGEN_DEVICE_FUNC 00798 SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType> 00799 ::computeDirect(const MatrixType& matrix, int options) 00800 { 00801 internal::direct_selfadjoint_eigenvalues<SelfAdjointEigenSolver,Size,NumTraits<Scalar>::IsComplex>::run(*this,matrix,options); 00802 return *this; 00803 } 00804 00805 namespace internal { 00806 template<int StorageOrder,typename RealScalar, typename Scalar, typename Index> 00807 EIGEN_DEVICE_FUNC 00808 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n) 00809 { 00810 using std::abs; 00811 RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5); 00812 RealScalar e = subdiag[end-1]; 00813 // Note that thanks to scaling, e^2 or td^2 cannot overflow, however they can still 00814 // underflow thus leading to inf/NaN values when using the following commented code: 00815 // RealScalar e2 = numext::abs2(subdiag[end-1]); 00816 // RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2)); 00817 // This explain the following, somewhat more complicated, version: 00818 RealScalar mu = diag[end]; 00819 if(td==RealScalar(0)) 00820 mu -= abs(e); 00821 else 00822 { 00823 RealScalar e2 = numext::abs2(subdiag[end-1]); 00824 RealScalar h = numext::hypot(td,e); 00825 if(e2==RealScalar(0)) mu -= (e / (td + (td>RealScalar(0) ? RealScalar(1) : RealScalar(-1)))) * (e / h); 00826 else mu -= e2 / (td + (td>RealScalar(0) ? h : -h)); 00827 } 00828 00829 RealScalar x = diag[start] - mu; 00830 RealScalar z = subdiag[start]; 00831 for (Index k = start; k < end; ++k) 00832 { 00833 JacobiRotation<RealScalar> rot; 00834 rot.makeGivens(x, z); 00835 00836 // do T = G' T G 00837 RealScalar sdk = rot.s() * diag[k] + rot.c() * subdiag[k]; 00838 RealScalar dkp1 = rot.s() * subdiag[k] + rot.c() * diag[k+1]; 00839 00840 diag[k] = rot.c() * (rot.c() * diag[k] - rot.s() * subdiag[k]) - rot.s() * (rot.c() * subdiag[k] - rot.s() * diag[k+1]); 00841 diag[k+1] = rot.s() * sdk + rot.c() * dkp1; 00842 subdiag[k] = rot.c() * sdk - rot.s() * dkp1; 00843 00844 00845 if (k > start) 00846 subdiag[k - 1] = rot.c() * subdiag[k-1] - rot.s() * z; 00847 00848 x = subdiag[k]; 00849 00850 if (k < end - 1) 00851 { 00852 z = -rot.s() * subdiag[k+1]; 00853 subdiag[k + 1] = rot.c() * subdiag[k+1]; 00854 } 00855 00856 // apply the givens rotation to the unit matrix Q = Q * G 00857 if (matrixQ) 00858 { 00859 // FIXME if StorageOrder == RowMajor this operation is not very efficient 00860 Map<Matrix<Scalar,Dynamic,Dynamic,StorageOrder> > q(matrixQ,n,n); 00861 q.applyOnTheRight(k,k+1,rot); 00862 } 00863 } 00864 } 00865 00866 } // end namespace internal 00867 00868 } // end namespace Eigen 00869 00870 #endif // EIGEN_SELFADJOINTEIGENSOLVER_H