LDLT.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008-2011 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 // Copyright (C) 2009 Keir Mierle <mierle@gmail.com>
00006 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
00007 // Copyright (C) 2011 Timothy E. Holy <tim.holy@gmail.com >
00008 //
00009 // This Source Code Form is subject to the terms of the Mozilla
00010 // Public License v. 2.0. If a copy of the MPL was not distributed
00011 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00012 
00013 #ifndef EIGEN_LDLT_H
00014 #define EIGEN_LDLT_H
00015 
00016 namespace Eigen { 
00017 
00018 namespace internal {
00019 template<typename MatrixType, int UpLo> struct LDLT_Traits;
00020 }
00021 
00045 template<typename _MatrixType, int _UpLo> class LDLT
00046 {
00047   public:
00048     typedef _MatrixType MatrixType;
00049     enum {
00050       RowsAtCompileTime = MatrixType::RowsAtCompileTime,
00051       ColsAtCompileTime = MatrixType::ColsAtCompileTime,
00052       Options = MatrixType::Options & ~RowMajorBit, // these are the options for the TmpMatrixType, we need a ColMajor matrix here!
00053       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
00054       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
00055       UpLo = _UpLo
00056     };
00057     typedef typename MatrixType::Scalar Scalar;
00058     typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
00059     typedef typename MatrixType::Index Index;
00060     typedef Matrix<Scalar, RowsAtCompileTime, 1, Options, MaxRowsAtCompileTime, 1> TmpMatrixType;
00061 
00062     typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
00063     typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType;
00064 
00065     typedef internal::LDLT_Traits<MatrixType,UpLo> Traits;
00066 
00072     LDLT() : m_matrix(), m_transpositions(), m_isInitialized(false) {}
00073 
00080     LDLT(Index size)
00081       : m_matrix(size, size),
00082         m_transpositions(size),
00083         m_temporary(size),
00084         m_isInitialized(false)
00085     {}
00086 
00092     LDLT(const MatrixType& matrix)
00093       : m_matrix(matrix.rows(), matrix.cols()),
00094         m_transpositions(matrix.rows()),
00095         m_temporary(matrix.rows()),
00096         m_isInitialized(false)
00097     {
00098       compute(matrix);
00099     }
00100 
00104     void setZero()
00105     {
00106       m_isInitialized = false;
00107     }
00108 
00110     inline typename Traits::MatrixU matrixU() const
00111     {
00112       eigen_assert(m_isInitialized && "LDLT is not initialized.");
00113       return Traits::getU(m_matrix);
00114     }
00115 
00117     inline typename Traits::MatrixL matrixL() const
00118     {
00119       eigen_assert(m_isInitialized && "LDLT is not initialized.");
00120       return Traits::getL(m_matrix);
00121     }
00122 
00125     inline const TranspositionType& transpositionsP() const
00126     {
00127       eigen_assert(m_isInitialized && "LDLT is not initialized.");
00128       return m_transpositions;
00129     }
00130 
00132     inline Diagonal<const MatrixType> vectorD() const
00133     {
00134       eigen_assert(m_isInitialized && "LDLT is not initialized.");
00135       return m_matrix.diagonal();
00136     }
00137 
00139     inline bool isPositive() const
00140     {
00141       eigen_assert(m_isInitialized && "LDLT is not initialized.");
00142       return m_sign == 1;
00143     }
00144     
00145     #ifdef EIGEN2_SUPPORT
00146     inline bool isPositiveDefinite() const
00147     {
00148       return isPositive();
00149     }
00150     #endif
00151 
00153     inline bool isNegative(void) const
00154     {
00155       eigen_assert(m_isInitialized && "LDLT is not initialized.");
00156       return m_sign == -1;
00157     }
00158 
00174     template<typename Rhs>
00175     inline const internal::solve_retval<LDLT, Rhs>
00176     solve(const MatrixBase<Rhs>& b) const
00177     {
00178       eigen_assert(m_isInitialized && "LDLT is not initialized.");
00179       eigen_assert(m_matrix.rows()==b.rows()
00180                 && "LDLT::solve(): invalid number of rows of the right hand side matrix b");
00181       return internal::solve_retval<LDLT, Rhs>(*this, b.derived());
00182     }
00183 
00184     #ifdef EIGEN2_SUPPORT
00185     template<typename OtherDerived, typename ResultType>
00186     bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const
00187     {
00188       *result = this->solve(b);
00189       return true;
00190     }
00191     #endif
00192 
00193     template<typename Derived>
00194     bool solveInPlace(MatrixBase<Derived> &bAndX) const;
00195 
00196     LDLT& compute(const MatrixType& matrix);
00197 
00198     template <typename Derived>
00199     LDLT& rankUpdate(const MatrixBase<Derived>& w, const RealScalar& alpha=1);
00200 
00205     inline const MatrixType& matrixLDLT() const
00206     {
00207       eigen_assert(m_isInitialized && "LDLT is not initialized.");
00208       return m_matrix;
00209     }
00210 
00211     MatrixType reconstructedMatrix() const;
00212 
00213     inline Index rows() const { return m_matrix.rows(); }
00214     inline Index cols() const { return m_matrix.cols(); }
00215 
00221     ComputationInfo info() const
00222     {
00223       eigen_assert(m_isInitialized && "LDLT is not initialized.");
00224       return Success;
00225     }
00226 
00227   protected:
00228 
00235     MatrixType m_matrix;
00236     TranspositionType m_transpositions;
00237     TmpMatrixType m_temporary;
00238     int m_sign;
00239     bool m_isInitialized;
00240 };
00241 
00242 namespace internal {
00243 
00244 template<int UpLo> struct ldlt_inplace;
00245 
00246 template<> struct ldlt_inplace<Lower>
00247 {
00248   template<typename MatrixType, typename TranspositionType, typename Workspace>
00249   static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
00250   {
00251     using std::abs;
00252     typedef typename MatrixType::Scalar Scalar;
00253     typedef typename MatrixType::RealScalar RealScalar;
00254     typedef typename MatrixType::Index Index;
00255     eigen_assert(mat.rows()==mat.cols());
00256     const Index size = mat.rows();
00257 
00258     if (size <= 1)
00259     {
00260       transpositions.setIdentity();
00261       if(sign)
00262         *sign = numext::real(mat.coeff(0,0))>0 ? 1:-1;
00263       return true;
00264     }
00265 
00266     RealScalar cutoff(0), biggest_in_corner;
00267 
00268     for (Index k = 0; k < size; ++k)
00269     {
00270       // Find largest diagonal element
00271       Index index_of_biggest_in_corner;
00272       biggest_in_corner = mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
00273       index_of_biggest_in_corner += k;
00274 
00275       if(k == 0)
00276       {
00277         // The biggest overall is the point of reference to which further diagonals
00278         // are compared; if any diagonal is negligible compared
00279         // to the largest overall, the algorithm bails.
00280         cutoff = abs(NumTraits<Scalar>::epsilon() * biggest_in_corner);
00281       }
00282 
00283       // Finish early if the matrix is not full rank.
00284       if(biggest_in_corner < cutoff)
00285       {
00286         for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i;
00287         if(sign) *sign = 0;
00288         break;
00289       }
00290 
00291       transpositions.coeffRef(k) = index_of_biggest_in_corner;
00292       if(k != index_of_biggest_in_corner)
00293       {
00294         // apply the transposition while taking care to consider only
00295         // the lower triangular part
00296         Index s = size-index_of_biggest_in_corner-1; // trailing size after the biggest element
00297         mat.row(k).head(k).swap(mat.row(index_of_biggest_in_corner).head(k));
00298         mat.col(k).tail(s).swap(mat.col(index_of_biggest_in_corner).tail(s));
00299         std::swap(mat.coeffRef(k,k),mat.coeffRef(index_of_biggest_in_corner,index_of_biggest_in_corner));
00300         for(int i=k+1;i<index_of_biggest_in_corner;++i)
00301         {
00302           Scalar tmp = mat.coeffRef(i,k);
00303           mat.coeffRef(i,k) = numext::conj(mat.coeffRef(index_of_biggest_in_corner,i));
00304           mat.coeffRef(index_of_biggest_in_corner,i) = numext::conj(tmp);
00305         }
00306         if(NumTraits<Scalar>::IsComplex)
00307           mat.coeffRef(index_of_biggest_in_corner,k) = numext::conj(mat.coeff(index_of_biggest_in_corner,k));
00308       }
00309 
00310       // partition the matrix:
00311       //       A00 |  -  |  -
00312       // lu  = A10 | A11 |  -
00313       //       A20 | A21 | A22
00314       Index rs = size - k - 1;
00315       Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1);
00316       Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k);
00317       Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k);
00318 
00319       if(k>0)
00320       {
00321         temp.head(k) = mat.diagonal().head(k).asDiagonal() * A10.adjoint();
00322         mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
00323         if(rs>0)
00324           A21.noalias() -= A20 * temp.head(k);
00325       }
00326       if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff))
00327         A21 /= mat.coeffRef(k,k);
00328       
00329       if(sign)
00330       {
00331         // LDLT is not guaranteed to work for indefinite matrices, but let's try to get the sign right
00332         int newSign = numext::real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0;
00333         if(k == 0)
00334           *sign = newSign;
00335         else if(*sign != newSign)
00336           *sign = 0;
00337       }
00338     }
00339 
00340     return true;
00341   }
00342 
00343   // Reference for the algorithm: Davis and Hager, "Multiple Rank
00344   // Modifications of a Sparse Cholesky Factorization" (Algorithm 1)
00345   // Trivial rearrangements of their computations (Timothy E. Holy)
00346   // allow their algorithm to work for rank-1 updates even if the
00347   // original matrix is not of full rank.
00348   // Here only rank-1 updates are implemented, to reduce the
00349   // requirement for intermediate storage and improve accuracy
00350   template<typename MatrixType, typename WDerived>
00351   static bool updateInPlace(MatrixType& mat, MatrixBase<WDerived>& w, const typename MatrixType::RealScalar& sigma=1)
00352   {
00353     using numext::isfinite;
00354     typedef typename MatrixType::Scalar Scalar;
00355     typedef typename MatrixType::RealScalar RealScalar;
00356     typedef typename MatrixType::Index Index;
00357 
00358     const Index size = mat.rows();
00359     eigen_assert(mat.cols() == size && w.size()==size);
00360 
00361     RealScalar alpha = 1;
00362 
00363     // Apply the update
00364     for (Index j = 0; j < size; j++)
00365     {
00366       // Check for termination due to an original decomposition of low-rank
00367       if (!(isfinite)(alpha))
00368         break;
00369 
00370       // Update the diagonal terms
00371       RealScalar dj = numext::real(mat.coeff(j,j));
00372       Scalar wj = w.coeff(j);
00373       RealScalar swj2 = sigma*numext::abs2(wj);
00374       RealScalar gamma = dj*alpha + swj2;
00375 
00376       mat.coeffRef(j,j) += swj2/alpha;
00377       alpha += swj2/dj;
00378 
00379 
00380       // Update the terms of L
00381       Index rs = size-j-1;
00382       w.tail(rs) -= wj * mat.col(j).tail(rs);
00383       if(gamma != 0)
00384         mat.col(j).tail(rs) += (sigma*numext::conj(wj)/gamma)*w.tail(rs);
00385     }
00386     return true;
00387   }
00388 
00389   template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
00390   static bool update(MatrixType& mat, const TranspositionType& transpositions, Workspace& tmp, const WType& w, const typename MatrixType::RealScalar& sigma=1)
00391   {
00392     // Apply the permutation to the input w
00393     tmp = transpositions * w;
00394 
00395     return ldlt_inplace<Lower>::updateInPlace(mat,tmp,sigma);
00396   }
00397 };
00398 
00399 template<> struct ldlt_inplace<Upper>
00400 {
00401   template<typename MatrixType, typename TranspositionType, typename Workspace>
00402   static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
00403   {
00404     Transpose<MatrixType> matt(mat);
00405     return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
00406   }
00407 
00408   template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType>
00409   static EIGEN_STRONG_INLINE bool update(MatrixType& mat, TranspositionType& transpositions, Workspace& tmp, WType& w, const typename MatrixType::RealScalar& sigma=1)
00410   {
00411     Transpose<MatrixType> matt(mat);
00412     return ldlt_inplace<Lower>::update(matt, transpositions, tmp, w.conjugate(), sigma);
00413   }
00414 };
00415 
00416 template<typename MatrixType> struct LDLT_Traits<MatrixType,Lower>
00417 {
00418   typedef const TriangularView<const MatrixType, UnitLower> MatrixL;
00419   typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitUpper> MatrixU;
00420   static inline MatrixL getL(const MatrixType& m) { return m; }
00421   static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); }
00422 };
00423 
00424 template<typename MatrixType> struct LDLT_Traits<MatrixType,Upper>
00425 {
00426   typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitLower> MatrixL;
00427   typedef const TriangularView<const MatrixType, UnitUpper> MatrixU;
00428   static inline MatrixL getL(const MatrixType& m) { return m.adjoint(); }
00429   static inline MatrixU getU(const MatrixType& m) { return m; }
00430 };
00431 
00432 } // end namespace internal
00433 
00436 template<typename MatrixType, int _UpLo>
00437 LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
00438 {
00439   eigen_assert(a.rows()==a.cols());
00440   const Index size = a.rows();
00441 
00442   m_matrix = a;
00443 
00444   m_transpositions.resize(size);
00445   m_isInitialized = false;
00446   m_temporary.resize(size);
00447 
00448   internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign);
00449 
00450   m_isInitialized = true;
00451   return *this;
00452 }
00453 
00459 template<typename MatrixType, int _UpLo>
00460 template<typename Derived>
00461 LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Derived>& w, const typename NumTraits<typename MatrixType::Scalar>::Real& sigma)
00462 {
00463   const Index size = w.rows();
00464   if (m_isInitialized)
00465   {
00466     eigen_assert(m_matrix.rows()==size);
00467   }
00468   else
00469   {    
00470     m_matrix.resize(size,size);
00471     m_matrix.setZero();
00472     m_transpositions.resize(size);
00473     for (Index i = 0; i < size; i++)
00474       m_transpositions.coeffRef(i) = i;
00475     m_temporary.resize(size);
00476     m_sign = sigma>=0 ? 1 : -1;
00477     m_isInitialized = true;
00478   }
00479 
00480   internal::ldlt_inplace<UpLo>::update(m_matrix, m_transpositions, m_temporary, w, sigma);
00481 
00482   return *this;
00483 }
00484 
00485 namespace internal {
00486 template<typename _MatrixType, int _UpLo, typename Rhs>
00487 struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
00488   : solve_retval_base<LDLT<_MatrixType,_UpLo>, Rhs>
00489 {
00490   typedef LDLT<_MatrixType,_UpLo> LDLTType;
00491   EIGEN_MAKE_SOLVE_HELPERS(LDLTType,Rhs)
00492 
00493   template<typename Dest> void evalTo(Dest& dst) const
00494   {
00495     eigen_assert(rhs().rows() == dec().matrixLDLT().rows());
00496     // dst = P b
00497     dst = dec().transpositionsP() * rhs();
00498 
00499     // dst = L^-1 (P b)
00500     dec().matrixL().solveInPlace(dst);
00501 
00502     // dst = D^-1 (L^-1 P b)
00503     // more precisely, use pseudo-inverse of D (see bug 241)
00504     using std::abs;
00505     using std::max;
00506     typedef typename LDLTType::MatrixType MatrixType;
00507     typedef typename LDLTType::Scalar Scalar;
00508     typedef typename LDLTType::RealScalar RealScalar;
00509     const Diagonal<const MatrixType> vectorD = dec().vectorD();
00510     RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits<Scalar>::epsilon(),
00511                                  RealScalar(1) / NumTraits<RealScalar>::highest()); // motivated by LAPACK's xGELSS
00512     for (Index i = 0; i < vectorD.size(); ++i) {
00513       if(abs(vectorD(i)) > tolerance)
00514         dst.row(i) /= vectorD(i);
00515       else
00516         dst.row(i).setZero();
00517     }
00518 
00519     // dst = L^-T (D^-1 L^-1 P b)
00520     dec().matrixU().solveInPlace(dst);
00521 
00522     // dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b
00523     dst = dec().transpositionsP().transpose() * dst;
00524   }
00525 };
00526 }
00527 
00541 template<typename MatrixType,int _UpLo>
00542 template<typename Derived>
00543 bool LDLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const
00544 {
00545   eigen_assert(m_isInitialized && "LDLT is not initialized.");
00546   eigen_assert(m_matrix.rows() == bAndX.rows());
00547 
00548   bAndX = this->solve(bAndX);
00549 
00550   return true;
00551 }
00552 
00556 template<typename MatrixType, int _UpLo>
00557 MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
00558 {
00559   eigen_assert(m_isInitialized && "LDLT is not initialized.");
00560   const Index size = m_matrix.rows();
00561   MatrixType res(size,size);
00562 
00563   // P
00564   res.setIdentity();
00565   res = transpositionsP() * res;
00566   // L^* P
00567   res = matrixU() * res;
00568   // D(L^*P)
00569   res = vectorD().asDiagonal() * res;
00570   // L(DL^*P)
00571   res = matrixL() * res;
00572   // P^T (LDL^*P)
00573   res = transpositionsP().transpose() * res;
00574 
00575   return res;
00576 }
00577 
00581 template<typename MatrixType, unsigned int UpLo>
00582 inline const LDLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo>
00583 SelfAdjointView<MatrixType, UpLo>::ldlt() const
00584 {
00585   return LDLT<PlainObject,UpLo>(m_matrix);
00586 }
00587 
00591 template<typename Derived>
00592 inline const LDLT<typename MatrixBase<Derived>::PlainObject>
00593 MatrixBase<Derived>::ldlt() const
00594 {
00595   return LDLT<PlainObject>(derived());
00596 }
00597 
00598 } // end namespace Eigen
00599 
00600 #endif // EIGEN_LDLT_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:37:51