Eigne Matrix Class Library
Dependents: MPC_current_control HydraulicControlBoard_SW AHRS Test_ekf ... more
src/Cholesky/LDLT.h@0:13a5d365ba16, 2016-10-13 (annotated)
- Committer:
- ykuroda
- Date:
- Thu Oct 13 04:07:23 2016 +0000
- Revision:
- 0:13a5d365ba16
First commint, Eigne Matrix Class Library
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ykuroda | 0:13a5d365ba16 | 1 | // This file is part of Eigen, a lightweight C++ template library |
ykuroda | 0:13a5d365ba16 | 2 | // for linear algebra. |
ykuroda | 0:13a5d365ba16 | 3 | // |
ykuroda | 0:13a5d365ba16 | 4 | // Copyright (C) 2008-2011 Gael Guennebaud <gael.guennebaud@inria.fr> |
ykuroda | 0:13a5d365ba16 | 5 | // Copyright (C) 2009 Keir Mierle <mierle@gmail.com> |
ykuroda | 0:13a5d365ba16 | 6 | // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> |
ykuroda | 0:13a5d365ba16 | 7 | // Copyright (C) 2011 Timothy E. Holy <tim.holy@gmail.com > |
ykuroda | 0:13a5d365ba16 | 8 | // |
ykuroda | 0:13a5d365ba16 | 9 | // This Source Code Form is subject to the terms of the Mozilla |
ykuroda | 0:13a5d365ba16 | 10 | // Public License v. 2.0. If a copy of the MPL was not distributed |
ykuroda | 0:13a5d365ba16 | 11 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. |
ykuroda | 0:13a5d365ba16 | 12 | |
ykuroda | 0:13a5d365ba16 | 13 | #ifndef EIGEN_LDLT_H |
ykuroda | 0:13a5d365ba16 | 14 | #define EIGEN_LDLT_H |
ykuroda | 0:13a5d365ba16 | 15 | |
ykuroda | 0:13a5d365ba16 | 16 | namespace Eigen { |
ykuroda | 0:13a5d365ba16 | 17 | |
ykuroda | 0:13a5d365ba16 | 18 | namespace internal { |
ykuroda | 0:13a5d365ba16 | 19 | template<typename MatrixType, int UpLo> struct LDLT_Traits; |
ykuroda | 0:13a5d365ba16 | 20 | |
ykuroda | 0:13a5d365ba16 | 21 | // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef |
ykuroda | 0:13a5d365ba16 | 22 | enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite }; |
ykuroda | 0:13a5d365ba16 | 23 | } |
ykuroda | 0:13a5d365ba16 | 24 | |
ykuroda | 0:13a5d365ba16 | 25 | /** \ingroup Cholesky_Module |
ykuroda | 0:13a5d365ba16 | 26 | * |
ykuroda | 0:13a5d365ba16 | 27 | * \class LDLT |
ykuroda | 0:13a5d365ba16 | 28 | * |
ykuroda | 0:13a5d365ba16 | 29 | * \brief Robust Cholesky decomposition of a matrix with pivoting |
ykuroda | 0:13a5d365ba16 | 30 | * |
ykuroda | 0:13a5d365ba16 | 31 | * \param MatrixType the type of the matrix of which to compute the LDL^T Cholesky decomposition |
ykuroda | 0:13a5d365ba16 | 32 | * \param UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper. |
ykuroda | 0:13a5d365ba16 | 33 | * The other triangular part won't be read. |
ykuroda | 0:13a5d365ba16 | 34 | * |
ykuroda | 0:13a5d365ba16 | 35 | * Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite |
ykuroda | 0:13a5d365ba16 | 36 | * matrix \f$ A \f$ such that \f$ A = P^TLDL^*P \f$, where P is a permutation matrix, L |
ykuroda | 0:13a5d365ba16 | 37 | * is lower triangular with a unit diagonal and D is a diagonal matrix. |
ykuroda | 0:13a5d365ba16 | 38 | * |
ykuroda | 0:13a5d365ba16 | 39 | * The decomposition uses pivoting to ensure stability, so that L will have |
ykuroda | 0:13a5d365ba16 | 40 | * zeros in the bottom right rank(A) - n submatrix. Avoiding the square root |
ykuroda | 0:13a5d365ba16 | 41 | * on D also stabilizes the computation. |
ykuroda | 0:13a5d365ba16 | 42 | * |
ykuroda | 0:13a5d365ba16 | 43 | * Remember that Cholesky decompositions are not rank-revealing. Also, do not use a Cholesky |
ykuroda | 0:13a5d365ba16 | 44 | * decomposition to determine whether a system of equations has a solution. |
ykuroda | 0:13a5d365ba16 | 45 | * |
ykuroda | 0:13a5d365ba16 | 46 | * \sa MatrixBase::ldlt(), class LLT |
ykuroda | 0:13a5d365ba16 | 47 | */ |
ykuroda | 0:13a5d365ba16 | 48 | template<typename _MatrixType, int _UpLo> class LDLT |
ykuroda | 0:13a5d365ba16 | 49 | { |
ykuroda | 0:13a5d365ba16 | 50 | public: |
ykuroda | 0:13a5d365ba16 | 51 | typedef _MatrixType MatrixType; |
ykuroda | 0:13a5d365ba16 | 52 | enum { |
ykuroda | 0:13a5d365ba16 | 53 | RowsAtCompileTime = MatrixType::RowsAtCompileTime, |
ykuroda | 0:13a5d365ba16 | 54 | ColsAtCompileTime = MatrixType::ColsAtCompileTime, |
ykuroda | 0:13a5d365ba16 | 55 | Options = MatrixType::Options & ~RowMajorBit, // these are the options for the TmpMatrixType, we need a ColMajor matrix here! |
ykuroda | 0:13a5d365ba16 | 56 | MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, |
ykuroda | 0:13a5d365ba16 | 57 | MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, |
ykuroda | 0:13a5d365ba16 | 58 | UpLo = _UpLo |
ykuroda | 0:13a5d365ba16 | 59 | }; |
ykuroda | 0:13a5d365ba16 | 60 | typedef typename MatrixType::Scalar Scalar; |
ykuroda | 0:13a5d365ba16 | 61 | typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar; |
ykuroda | 0:13a5d365ba16 | 62 | typedef typename MatrixType::Index Index; |
ykuroda | 0:13a5d365ba16 | 63 | typedef Matrix<Scalar, RowsAtCompileTime, 1, Options, MaxRowsAtCompileTime, 1> TmpMatrixType; |
ykuroda | 0:13a5d365ba16 | 64 | |
ykuroda | 0:13a5d365ba16 | 65 | typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType; |
ykuroda | 0:13a5d365ba16 | 66 | typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType; |
ykuroda | 0:13a5d365ba16 | 67 | |
ykuroda | 0:13a5d365ba16 | 68 | typedef internal::LDLT_Traits<MatrixType,UpLo> Traits; |
ykuroda | 0:13a5d365ba16 | 69 | |
ykuroda | 0:13a5d365ba16 | 70 | /** \brief Default Constructor. |
ykuroda | 0:13a5d365ba16 | 71 | * |
ykuroda | 0:13a5d365ba16 | 72 | * The default constructor is useful in cases in which the user intends to |
ykuroda | 0:13a5d365ba16 | 73 | * perform decompositions via LDLT::compute(const MatrixType&). |
ykuroda | 0:13a5d365ba16 | 74 | */ |
ykuroda | 0:13a5d365ba16 | 75 | LDLT() |
ykuroda | 0:13a5d365ba16 | 76 | : m_matrix(), |
ykuroda | 0:13a5d365ba16 | 77 | m_transpositions(), |
ykuroda | 0:13a5d365ba16 | 78 | m_sign(internal::ZeroSign), |
ykuroda | 0:13a5d365ba16 | 79 | m_isInitialized(false) |
ykuroda | 0:13a5d365ba16 | 80 | {} |
ykuroda | 0:13a5d365ba16 | 81 | |
ykuroda | 0:13a5d365ba16 | 82 | /** \brief Default Constructor with memory preallocation |
ykuroda | 0:13a5d365ba16 | 83 | * |
ykuroda | 0:13a5d365ba16 | 84 | * Like the default constructor but with preallocation of the internal data |
ykuroda | 0:13a5d365ba16 | 85 | * according to the specified problem \a size. |
ykuroda | 0:13a5d365ba16 | 86 | * \sa LDLT() |
ykuroda | 0:13a5d365ba16 | 87 | */ |
ykuroda | 0:13a5d365ba16 | 88 | LDLT(Index size) |
ykuroda | 0:13a5d365ba16 | 89 | : m_matrix(size, size), |
ykuroda | 0:13a5d365ba16 | 90 | m_transpositions(size), |
ykuroda | 0:13a5d365ba16 | 91 | m_temporary(size), |
ykuroda | 0:13a5d365ba16 | 92 | m_sign(internal::ZeroSign), |
ykuroda | 0:13a5d365ba16 | 93 | m_isInitialized(false) |
ykuroda | 0:13a5d365ba16 | 94 | {} |
ykuroda | 0:13a5d365ba16 | 95 | |
ykuroda | 0:13a5d365ba16 | 96 | /** \brief Constructor with decomposition |
ykuroda | 0:13a5d365ba16 | 97 | * |
ykuroda | 0:13a5d365ba16 | 98 | * This calculates the decomposition for the input \a matrix. |
ykuroda | 0:13a5d365ba16 | 99 | * \sa LDLT(Index size) |
ykuroda | 0:13a5d365ba16 | 100 | */ |
ykuroda | 0:13a5d365ba16 | 101 | LDLT(const MatrixType& matrix) |
ykuroda | 0:13a5d365ba16 | 102 | : m_matrix(matrix.rows(), matrix.cols()), |
ykuroda | 0:13a5d365ba16 | 103 | m_transpositions(matrix.rows()), |
ykuroda | 0:13a5d365ba16 | 104 | m_temporary(matrix.rows()), |
ykuroda | 0:13a5d365ba16 | 105 | m_sign(internal::ZeroSign), |
ykuroda | 0:13a5d365ba16 | 106 | m_isInitialized(false) |
ykuroda | 0:13a5d365ba16 | 107 | { |
ykuroda | 0:13a5d365ba16 | 108 | compute(matrix); |
ykuroda | 0:13a5d365ba16 | 109 | } |
ykuroda | 0:13a5d365ba16 | 110 | |
ykuroda | 0:13a5d365ba16 | 111 | /** Clear any existing decomposition |
ykuroda | 0:13a5d365ba16 | 112 | * \sa rankUpdate(w,sigma) |
ykuroda | 0:13a5d365ba16 | 113 | */ |
ykuroda | 0:13a5d365ba16 | 114 | void setZero() |
ykuroda | 0:13a5d365ba16 | 115 | { |
ykuroda | 0:13a5d365ba16 | 116 | m_isInitialized = false; |
ykuroda | 0:13a5d365ba16 | 117 | } |
ykuroda | 0:13a5d365ba16 | 118 | |
ykuroda | 0:13a5d365ba16 | 119 | /** \returns a view of the upper triangular matrix U */ |
ykuroda | 0:13a5d365ba16 | 120 | inline typename Traits::MatrixU matrixU() const |
ykuroda | 0:13a5d365ba16 | 121 | { |
ykuroda | 0:13a5d365ba16 | 122 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 123 | return Traits::getU(m_matrix); |
ykuroda | 0:13a5d365ba16 | 124 | } |
ykuroda | 0:13a5d365ba16 | 125 | |
ykuroda | 0:13a5d365ba16 | 126 | /** \returns a view of the lower triangular matrix L */ |
ykuroda | 0:13a5d365ba16 | 127 | inline typename Traits::MatrixL matrixL() const |
ykuroda | 0:13a5d365ba16 | 128 | { |
ykuroda | 0:13a5d365ba16 | 129 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 130 | return Traits::getL(m_matrix); |
ykuroda | 0:13a5d365ba16 | 131 | } |
ykuroda | 0:13a5d365ba16 | 132 | |
ykuroda | 0:13a5d365ba16 | 133 | /** \returns the permutation matrix P as a transposition sequence. |
ykuroda | 0:13a5d365ba16 | 134 | */ |
ykuroda | 0:13a5d365ba16 | 135 | inline const TranspositionType& transpositionsP() const |
ykuroda | 0:13a5d365ba16 | 136 | { |
ykuroda | 0:13a5d365ba16 | 137 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 138 | return m_transpositions; |
ykuroda | 0:13a5d365ba16 | 139 | } |
ykuroda | 0:13a5d365ba16 | 140 | |
ykuroda | 0:13a5d365ba16 | 141 | /** \returns the coefficients of the diagonal matrix D */ |
ykuroda | 0:13a5d365ba16 | 142 | inline Diagonal<const MatrixType> vectorD() const |
ykuroda | 0:13a5d365ba16 | 143 | { |
ykuroda | 0:13a5d365ba16 | 144 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 145 | return m_matrix.diagonal(); |
ykuroda | 0:13a5d365ba16 | 146 | } |
ykuroda | 0:13a5d365ba16 | 147 | |
ykuroda | 0:13a5d365ba16 | 148 | /** \returns true if the matrix is positive (semidefinite) */ |
ykuroda | 0:13a5d365ba16 | 149 | inline bool isPositive() const |
ykuroda | 0:13a5d365ba16 | 150 | { |
ykuroda | 0:13a5d365ba16 | 151 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 152 | return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign; |
ykuroda | 0:13a5d365ba16 | 153 | } |
ykuroda | 0:13a5d365ba16 | 154 | |
ykuroda | 0:13a5d365ba16 | 155 | #ifdef EIGEN2_SUPPORT |
ykuroda | 0:13a5d365ba16 | 156 | inline bool isPositiveDefinite() const |
ykuroda | 0:13a5d365ba16 | 157 | { |
ykuroda | 0:13a5d365ba16 | 158 | return isPositive(); |
ykuroda | 0:13a5d365ba16 | 159 | } |
ykuroda | 0:13a5d365ba16 | 160 | #endif |
ykuroda | 0:13a5d365ba16 | 161 | |
ykuroda | 0:13a5d365ba16 | 162 | /** \returns true if the matrix is negative (semidefinite) */ |
ykuroda | 0:13a5d365ba16 | 163 | inline bool isNegative(void) const |
ykuroda | 0:13a5d365ba16 | 164 | { |
ykuroda | 0:13a5d365ba16 | 165 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 166 | return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign; |
ykuroda | 0:13a5d365ba16 | 167 | } |
ykuroda | 0:13a5d365ba16 | 168 | |
ykuroda | 0:13a5d365ba16 | 169 | /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A. |
ykuroda | 0:13a5d365ba16 | 170 | * |
ykuroda | 0:13a5d365ba16 | 171 | * This function also supports in-place solves using the syntax <tt>x = decompositionObject.solve(x)</tt> . |
ykuroda | 0:13a5d365ba16 | 172 | * |
ykuroda | 0:13a5d365ba16 | 173 | * \note_about_checking_solutions |
ykuroda | 0:13a5d365ba16 | 174 | * |
ykuroda | 0:13a5d365ba16 | 175 | * More precisely, this method solves \f$ A x = b \f$ using the decomposition \f$ A = P^T L D L^* P \f$ |
ykuroda | 0:13a5d365ba16 | 176 | * by solving the systems \f$ P^T y_1 = b \f$, \f$ L y_2 = y_1 \f$, \f$ D y_3 = y_2 \f$, |
ykuroda | 0:13a5d365ba16 | 177 | * \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then |
ykuroda | 0:13a5d365ba16 | 178 | * \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the |
ykuroda | 0:13a5d365ba16 | 179 | * least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function |
ykuroda | 0:13a5d365ba16 | 180 | * computes the least-square solution of \f$ A x = b \f$ is \f$ A \f$ is singular. |
ykuroda | 0:13a5d365ba16 | 181 | * |
ykuroda | 0:13a5d365ba16 | 182 | * \sa MatrixBase::ldlt() |
ykuroda | 0:13a5d365ba16 | 183 | */ |
ykuroda | 0:13a5d365ba16 | 184 | template<typename Rhs> |
ykuroda | 0:13a5d365ba16 | 185 | inline const internal::solve_retval<LDLT, Rhs> |
ykuroda | 0:13a5d365ba16 | 186 | solve(const MatrixBase<Rhs>& b) const |
ykuroda | 0:13a5d365ba16 | 187 | { |
ykuroda | 0:13a5d365ba16 | 188 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 189 | eigen_assert(m_matrix.rows()==b.rows() |
ykuroda | 0:13a5d365ba16 | 190 | && "LDLT::solve(): invalid number of rows of the right hand side matrix b"); |
ykuroda | 0:13a5d365ba16 | 191 | return internal::solve_retval<LDLT, Rhs>(*this, b.derived()); |
ykuroda | 0:13a5d365ba16 | 192 | } |
ykuroda | 0:13a5d365ba16 | 193 | |
ykuroda | 0:13a5d365ba16 | 194 | #ifdef EIGEN2_SUPPORT |
ykuroda | 0:13a5d365ba16 | 195 | template<typename OtherDerived, typename ResultType> |
ykuroda | 0:13a5d365ba16 | 196 | bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const |
ykuroda | 0:13a5d365ba16 | 197 | { |
ykuroda | 0:13a5d365ba16 | 198 | *result = this->solve(b); |
ykuroda | 0:13a5d365ba16 | 199 | return true; |
ykuroda | 0:13a5d365ba16 | 200 | } |
ykuroda | 0:13a5d365ba16 | 201 | #endif |
ykuroda | 0:13a5d365ba16 | 202 | |
ykuroda | 0:13a5d365ba16 | 203 | template<typename Derived> |
ykuroda | 0:13a5d365ba16 | 204 | bool solveInPlace(MatrixBase<Derived> &bAndX) const; |
ykuroda | 0:13a5d365ba16 | 205 | |
ykuroda | 0:13a5d365ba16 | 206 | LDLT& compute(const MatrixType& matrix); |
ykuroda | 0:13a5d365ba16 | 207 | |
ykuroda | 0:13a5d365ba16 | 208 | template <typename Derived> |
ykuroda | 0:13a5d365ba16 | 209 | LDLT& rankUpdate(const MatrixBase<Derived>& w, const RealScalar& alpha=1); |
ykuroda | 0:13a5d365ba16 | 210 | |
ykuroda | 0:13a5d365ba16 | 211 | /** \returns the internal LDLT decomposition matrix |
ykuroda | 0:13a5d365ba16 | 212 | * |
ykuroda | 0:13a5d365ba16 | 213 | * TODO: document the storage layout |
ykuroda | 0:13a5d365ba16 | 214 | */ |
ykuroda | 0:13a5d365ba16 | 215 | inline const MatrixType& matrixLDLT() const |
ykuroda | 0:13a5d365ba16 | 216 | { |
ykuroda | 0:13a5d365ba16 | 217 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 218 | return m_matrix; |
ykuroda | 0:13a5d365ba16 | 219 | } |
ykuroda | 0:13a5d365ba16 | 220 | |
ykuroda | 0:13a5d365ba16 | 221 | MatrixType reconstructedMatrix() const; |
ykuroda | 0:13a5d365ba16 | 222 | |
ykuroda | 0:13a5d365ba16 | 223 | inline Index rows() const { return m_matrix.rows(); } |
ykuroda | 0:13a5d365ba16 | 224 | inline Index cols() const { return m_matrix.cols(); } |
ykuroda | 0:13a5d365ba16 | 225 | |
ykuroda | 0:13a5d365ba16 | 226 | /** \brief Reports whether previous computation was successful. |
ykuroda | 0:13a5d365ba16 | 227 | * |
ykuroda | 0:13a5d365ba16 | 228 | * \returns \c Success if computation was succesful, |
ykuroda | 0:13a5d365ba16 | 229 | * \c NumericalIssue if the matrix.appears to be negative. |
ykuroda | 0:13a5d365ba16 | 230 | */ |
ykuroda | 0:13a5d365ba16 | 231 | ComputationInfo info() const |
ykuroda | 0:13a5d365ba16 | 232 | { |
ykuroda | 0:13a5d365ba16 | 233 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 234 | return Success; |
ykuroda | 0:13a5d365ba16 | 235 | } |
ykuroda | 0:13a5d365ba16 | 236 | |
ykuroda | 0:13a5d365ba16 | 237 | protected: |
ykuroda | 0:13a5d365ba16 | 238 | |
ykuroda | 0:13a5d365ba16 | 239 | static void check_template_parameters() |
ykuroda | 0:13a5d365ba16 | 240 | { |
ykuroda | 0:13a5d365ba16 | 241 | EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); |
ykuroda | 0:13a5d365ba16 | 242 | } |
ykuroda | 0:13a5d365ba16 | 243 | |
ykuroda | 0:13a5d365ba16 | 244 | /** \internal |
ykuroda | 0:13a5d365ba16 | 245 | * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U. |
ykuroda | 0:13a5d365ba16 | 246 | * The strict upper part is used during the decomposition, the strict lower |
ykuroda | 0:13a5d365ba16 | 247 | * part correspond to the coefficients of L (its diagonal is equal to 1 and |
ykuroda | 0:13a5d365ba16 | 248 | * is not stored), and the diagonal entries correspond to D. |
ykuroda | 0:13a5d365ba16 | 249 | */ |
ykuroda | 0:13a5d365ba16 | 250 | MatrixType m_matrix; |
ykuroda | 0:13a5d365ba16 | 251 | TranspositionType m_transpositions; |
ykuroda | 0:13a5d365ba16 | 252 | TmpMatrixType m_temporary; |
ykuroda | 0:13a5d365ba16 | 253 | internal::SignMatrix m_sign; |
ykuroda | 0:13a5d365ba16 | 254 | bool m_isInitialized; |
ykuroda | 0:13a5d365ba16 | 255 | }; |
ykuroda | 0:13a5d365ba16 | 256 | |
ykuroda | 0:13a5d365ba16 | 257 | namespace internal { |
ykuroda | 0:13a5d365ba16 | 258 | |
ykuroda | 0:13a5d365ba16 | 259 | template<int UpLo> struct ldlt_inplace; |
ykuroda | 0:13a5d365ba16 | 260 | |
ykuroda | 0:13a5d365ba16 | 261 | template<> struct ldlt_inplace<Lower> |
ykuroda | 0:13a5d365ba16 | 262 | { |
ykuroda | 0:13a5d365ba16 | 263 | template<typename MatrixType, typename TranspositionType, typename Workspace> |
ykuroda | 0:13a5d365ba16 | 264 | static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) |
ykuroda | 0:13a5d365ba16 | 265 | { |
ykuroda | 0:13a5d365ba16 | 266 | using std::abs; |
ykuroda | 0:13a5d365ba16 | 267 | typedef typename MatrixType::Scalar Scalar; |
ykuroda | 0:13a5d365ba16 | 268 | typedef typename MatrixType::RealScalar RealScalar; |
ykuroda | 0:13a5d365ba16 | 269 | typedef typename MatrixType::Index Index; |
ykuroda | 0:13a5d365ba16 | 270 | eigen_assert(mat.rows()==mat.cols()); |
ykuroda | 0:13a5d365ba16 | 271 | const Index size = mat.rows(); |
ykuroda | 0:13a5d365ba16 | 272 | |
ykuroda | 0:13a5d365ba16 | 273 | if (size <= 1) |
ykuroda | 0:13a5d365ba16 | 274 | { |
ykuroda | 0:13a5d365ba16 | 275 | transpositions.setIdentity(); |
ykuroda | 0:13a5d365ba16 | 276 | if (numext::real(mat.coeff(0,0)) > 0) sign = PositiveSemiDef; |
ykuroda | 0:13a5d365ba16 | 277 | else if (numext::real(mat.coeff(0,0)) < 0) sign = NegativeSemiDef; |
ykuroda | 0:13a5d365ba16 | 278 | else sign = ZeroSign; |
ykuroda | 0:13a5d365ba16 | 279 | return true; |
ykuroda | 0:13a5d365ba16 | 280 | } |
ykuroda | 0:13a5d365ba16 | 281 | |
ykuroda | 0:13a5d365ba16 | 282 | for (Index k = 0; k < size; ++k) |
ykuroda | 0:13a5d365ba16 | 283 | { |
ykuroda | 0:13a5d365ba16 | 284 | // Find largest diagonal element |
ykuroda | 0:13a5d365ba16 | 285 | Index index_of_biggest_in_corner; |
ykuroda | 0:13a5d365ba16 | 286 | mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner); |
ykuroda | 0:13a5d365ba16 | 287 | index_of_biggest_in_corner += k; |
ykuroda | 0:13a5d365ba16 | 288 | |
ykuroda | 0:13a5d365ba16 | 289 | transpositions.coeffRef(k) = index_of_biggest_in_corner; |
ykuroda | 0:13a5d365ba16 | 290 | if(k != index_of_biggest_in_corner) |
ykuroda | 0:13a5d365ba16 | 291 | { |
ykuroda | 0:13a5d365ba16 | 292 | // apply the transposition while taking care to consider only |
ykuroda | 0:13a5d365ba16 | 293 | // the lower triangular part |
ykuroda | 0:13a5d365ba16 | 294 | Index s = size-index_of_biggest_in_corner-1; // trailing size after the biggest element |
ykuroda | 0:13a5d365ba16 | 295 | mat.row(k).head(k).swap(mat.row(index_of_biggest_in_corner).head(k)); |
ykuroda | 0:13a5d365ba16 | 296 | mat.col(k).tail(s).swap(mat.col(index_of_biggest_in_corner).tail(s)); |
ykuroda | 0:13a5d365ba16 | 297 | std::swap(mat.coeffRef(k,k),mat.coeffRef(index_of_biggest_in_corner,index_of_biggest_in_corner)); |
ykuroda | 0:13a5d365ba16 | 298 | for(int i=k+1;i<index_of_biggest_in_corner;++i) |
ykuroda | 0:13a5d365ba16 | 299 | { |
ykuroda | 0:13a5d365ba16 | 300 | Scalar tmp = mat.coeffRef(i,k); |
ykuroda | 0:13a5d365ba16 | 301 | mat.coeffRef(i,k) = numext::conj(mat.coeffRef(index_of_biggest_in_corner,i)); |
ykuroda | 0:13a5d365ba16 | 302 | mat.coeffRef(index_of_biggest_in_corner,i) = numext::conj(tmp); |
ykuroda | 0:13a5d365ba16 | 303 | } |
ykuroda | 0:13a5d365ba16 | 304 | if(NumTraits<Scalar>::IsComplex) |
ykuroda | 0:13a5d365ba16 | 305 | mat.coeffRef(index_of_biggest_in_corner,k) = numext::conj(mat.coeff(index_of_biggest_in_corner,k)); |
ykuroda | 0:13a5d365ba16 | 306 | } |
ykuroda | 0:13a5d365ba16 | 307 | |
ykuroda | 0:13a5d365ba16 | 308 | // partition the matrix: |
ykuroda | 0:13a5d365ba16 | 309 | // A00 | - | - |
ykuroda | 0:13a5d365ba16 | 310 | // lu = A10 | A11 | - |
ykuroda | 0:13a5d365ba16 | 311 | // A20 | A21 | A22 |
ykuroda | 0:13a5d365ba16 | 312 | Index rs = size - k - 1; |
ykuroda | 0:13a5d365ba16 | 313 | Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1); |
ykuroda | 0:13a5d365ba16 | 314 | Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k); |
ykuroda | 0:13a5d365ba16 | 315 | Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k); |
ykuroda | 0:13a5d365ba16 | 316 | |
ykuroda | 0:13a5d365ba16 | 317 | if(k>0) |
ykuroda | 0:13a5d365ba16 | 318 | { |
ykuroda | 0:13a5d365ba16 | 319 | temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint(); |
ykuroda | 0:13a5d365ba16 | 320 | mat.coeffRef(k,k) -= (A10 * temp.head(k)).value(); |
ykuroda | 0:13a5d365ba16 | 321 | if(rs>0) |
ykuroda | 0:13a5d365ba16 | 322 | A21.noalias() -= A20 * temp.head(k); |
ykuroda | 0:13a5d365ba16 | 323 | } |
ykuroda | 0:13a5d365ba16 | 324 | |
ykuroda | 0:13a5d365ba16 | 325 | // In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot |
ykuroda | 0:13a5d365ba16 | 326 | // was smaller than the cutoff value. However, soince LDLT is not rank-revealing |
ykuroda | 0:13a5d365ba16 | 327 | // we should only make sure we do not introduce INF or NaN values. |
ykuroda | 0:13a5d365ba16 | 328 | // LAPACK also uses 0 as the cutoff value. |
ykuroda | 0:13a5d365ba16 | 329 | RealScalar realAkk = numext::real(mat.coeffRef(k,k)); |
ykuroda | 0:13a5d365ba16 | 330 | if((rs>0) && (abs(realAkk) > RealScalar(0))) |
ykuroda | 0:13a5d365ba16 | 331 | A21 /= realAkk; |
ykuroda | 0:13a5d365ba16 | 332 | |
ykuroda | 0:13a5d365ba16 | 333 | if (sign == PositiveSemiDef) { |
ykuroda | 0:13a5d365ba16 | 334 | if (realAkk < 0) sign = Indefinite; |
ykuroda | 0:13a5d365ba16 | 335 | } else if (sign == NegativeSemiDef) { |
ykuroda | 0:13a5d365ba16 | 336 | if (realAkk > 0) sign = Indefinite; |
ykuroda | 0:13a5d365ba16 | 337 | } else if (sign == ZeroSign) { |
ykuroda | 0:13a5d365ba16 | 338 | if (realAkk > 0) sign = PositiveSemiDef; |
ykuroda | 0:13a5d365ba16 | 339 | else if (realAkk < 0) sign = NegativeSemiDef; |
ykuroda | 0:13a5d365ba16 | 340 | } |
ykuroda | 0:13a5d365ba16 | 341 | } |
ykuroda | 0:13a5d365ba16 | 342 | |
ykuroda | 0:13a5d365ba16 | 343 | return true; |
ykuroda | 0:13a5d365ba16 | 344 | } |
ykuroda | 0:13a5d365ba16 | 345 | |
ykuroda | 0:13a5d365ba16 | 346 | // Reference for the algorithm: Davis and Hager, "Multiple Rank |
ykuroda | 0:13a5d365ba16 | 347 | // Modifications of a Sparse Cholesky Factorization" (Algorithm 1) |
ykuroda | 0:13a5d365ba16 | 348 | // Trivial rearrangements of their computations (Timothy E. Holy) |
ykuroda | 0:13a5d365ba16 | 349 | // allow their algorithm to work for rank-1 updates even if the |
ykuroda | 0:13a5d365ba16 | 350 | // original matrix is not of full rank. |
ykuroda | 0:13a5d365ba16 | 351 | // Here only rank-1 updates are implemented, to reduce the |
ykuroda | 0:13a5d365ba16 | 352 | // requirement for intermediate storage and improve accuracy |
ykuroda | 0:13a5d365ba16 | 353 | template<typename MatrixType, typename WDerived> |
ykuroda | 0:13a5d365ba16 | 354 | static bool updateInPlace(MatrixType& mat, MatrixBase<WDerived>& w, const typename MatrixType::RealScalar& sigma=1) |
ykuroda | 0:13a5d365ba16 | 355 | { |
ykuroda | 0:13a5d365ba16 | 356 | using numext::isfinite; |
ykuroda | 0:13a5d365ba16 | 357 | typedef typename MatrixType::Scalar Scalar; |
ykuroda | 0:13a5d365ba16 | 358 | typedef typename MatrixType::RealScalar RealScalar; |
ykuroda | 0:13a5d365ba16 | 359 | typedef typename MatrixType::Index Index; |
ykuroda | 0:13a5d365ba16 | 360 | |
ykuroda | 0:13a5d365ba16 | 361 | const Index size = mat.rows(); |
ykuroda | 0:13a5d365ba16 | 362 | eigen_assert(mat.cols() == size && w.size()==size); |
ykuroda | 0:13a5d365ba16 | 363 | |
ykuroda | 0:13a5d365ba16 | 364 | RealScalar alpha = 1; |
ykuroda | 0:13a5d365ba16 | 365 | |
ykuroda | 0:13a5d365ba16 | 366 | // Apply the update |
ykuroda | 0:13a5d365ba16 | 367 | for (Index j = 0; j < size; j++) |
ykuroda | 0:13a5d365ba16 | 368 | { |
ykuroda | 0:13a5d365ba16 | 369 | // Check for termination due to an original decomposition of low-rank |
ykuroda | 0:13a5d365ba16 | 370 | if (!(isfinite)(alpha)) |
ykuroda | 0:13a5d365ba16 | 371 | break; |
ykuroda | 0:13a5d365ba16 | 372 | |
ykuroda | 0:13a5d365ba16 | 373 | // Update the diagonal terms |
ykuroda | 0:13a5d365ba16 | 374 | RealScalar dj = numext::real(mat.coeff(j,j)); |
ykuroda | 0:13a5d365ba16 | 375 | Scalar wj = w.coeff(j); |
ykuroda | 0:13a5d365ba16 | 376 | RealScalar swj2 = sigma*numext::abs2(wj); |
ykuroda | 0:13a5d365ba16 | 377 | RealScalar gamma = dj*alpha + swj2; |
ykuroda | 0:13a5d365ba16 | 378 | |
ykuroda | 0:13a5d365ba16 | 379 | mat.coeffRef(j,j) += swj2/alpha; |
ykuroda | 0:13a5d365ba16 | 380 | alpha += swj2/dj; |
ykuroda | 0:13a5d365ba16 | 381 | |
ykuroda | 0:13a5d365ba16 | 382 | |
ykuroda | 0:13a5d365ba16 | 383 | // Update the terms of L |
ykuroda | 0:13a5d365ba16 | 384 | Index rs = size-j-1; |
ykuroda | 0:13a5d365ba16 | 385 | w.tail(rs) -= wj * mat.col(j).tail(rs); |
ykuroda | 0:13a5d365ba16 | 386 | if(gamma != 0) |
ykuroda | 0:13a5d365ba16 | 387 | mat.col(j).tail(rs) += (sigma*numext::conj(wj)/gamma)*w.tail(rs); |
ykuroda | 0:13a5d365ba16 | 388 | } |
ykuroda | 0:13a5d365ba16 | 389 | return true; |
ykuroda | 0:13a5d365ba16 | 390 | } |
ykuroda | 0:13a5d365ba16 | 391 | |
ykuroda | 0:13a5d365ba16 | 392 | template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType> |
ykuroda | 0:13a5d365ba16 | 393 | static bool update(MatrixType& mat, const TranspositionType& transpositions, Workspace& tmp, const WType& w, const typename MatrixType::RealScalar& sigma=1) |
ykuroda | 0:13a5d365ba16 | 394 | { |
ykuroda | 0:13a5d365ba16 | 395 | // Apply the permutation to the input w |
ykuroda | 0:13a5d365ba16 | 396 | tmp = transpositions * w; |
ykuroda | 0:13a5d365ba16 | 397 | |
ykuroda | 0:13a5d365ba16 | 398 | return ldlt_inplace<Lower>::updateInPlace(mat,tmp,sigma); |
ykuroda | 0:13a5d365ba16 | 399 | } |
ykuroda | 0:13a5d365ba16 | 400 | }; |
ykuroda | 0:13a5d365ba16 | 401 | |
ykuroda | 0:13a5d365ba16 | 402 | template<> struct ldlt_inplace<Upper> |
ykuroda | 0:13a5d365ba16 | 403 | { |
ykuroda | 0:13a5d365ba16 | 404 | template<typename MatrixType, typename TranspositionType, typename Workspace> |
ykuroda | 0:13a5d365ba16 | 405 | static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) |
ykuroda | 0:13a5d365ba16 | 406 | { |
ykuroda | 0:13a5d365ba16 | 407 | Transpose<MatrixType> matt(mat); |
ykuroda | 0:13a5d365ba16 | 408 | return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign); |
ykuroda | 0:13a5d365ba16 | 409 | } |
ykuroda | 0:13a5d365ba16 | 410 | |
ykuroda | 0:13a5d365ba16 | 411 | template<typename MatrixType, typename TranspositionType, typename Workspace, typename WType> |
ykuroda | 0:13a5d365ba16 | 412 | static EIGEN_STRONG_INLINE bool update(MatrixType& mat, TranspositionType& transpositions, Workspace& tmp, WType& w, const typename MatrixType::RealScalar& sigma=1) |
ykuroda | 0:13a5d365ba16 | 413 | { |
ykuroda | 0:13a5d365ba16 | 414 | Transpose<MatrixType> matt(mat); |
ykuroda | 0:13a5d365ba16 | 415 | return ldlt_inplace<Lower>::update(matt, transpositions, tmp, w.conjugate(), sigma); |
ykuroda | 0:13a5d365ba16 | 416 | } |
ykuroda | 0:13a5d365ba16 | 417 | }; |
ykuroda | 0:13a5d365ba16 | 418 | |
ykuroda | 0:13a5d365ba16 | 419 | template<typename MatrixType> struct LDLT_Traits<MatrixType,Lower> |
ykuroda | 0:13a5d365ba16 | 420 | { |
ykuroda | 0:13a5d365ba16 | 421 | typedef const TriangularView<const MatrixType, UnitLower> MatrixL; |
ykuroda | 0:13a5d365ba16 | 422 | typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitUpper> MatrixU; |
ykuroda | 0:13a5d365ba16 | 423 | static inline MatrixL getL(const MatrixType& m) { return m; } |
ykuroda | 0:13a5d365ba16 | 424 | static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); } |
ykuroda | 0:13a5d365ba16 | 425 | }; |
ykuroda | 0:13a5d365ba16 | 426 | |
ykuroda | 0:13a5d365ba16 | 427 | template<typename MatrixType> struct LDLT_Traits<MatrixType,Upper> |
ykuroda | 0:13a5d365ba16 | 428 | { |
ykuroda | 0:13a5d365ba16 | 429 | typedef const TriangularView<const typename MatrixType::AdjointReturnType, UnitLower> MatrixL; |
ykuroda | 0:13a5d365ba16 | 430 | typedef const TriangularView<const MatrixType, UnitUpper> MatrixU; |
ykuroda | 0:13a5d365ba16 | 431 | static inline MatrixL getL(const MatrixType& m) { return m.adjoint(); } |
ykuroda | 0:13a5d365ba16 | 432 | static inline MatrixU getU(const MatrixType& m) { return m; } |
ykuroda | 0:13a5d365ba16 | 433 | }; |
ykuroda | 0:13a5d365ba16 | 434 | |
ykuroda | 0:13a5d365ba16 | 435 | } // end namespace internal |
ykuroda | 0:13a5d365ba16 | 436 | |
ykuroda | 0:13a5d365ba16 | 437 | /** Compute / recompute the LDLT decomposition A = L D L^* = U^* D U of \a matrix |
ykuroda | 0:13a5d365ba16 | 438 | */ |
ykuroda | 0:13a5d365ba16 | 439 | template<typename MatrixType, int _UpLo> |
ykuroda | 0:13a5d365ba16 | 440 | LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a) |
ykuroda | 0:13a5d365ba16 | 441 | { |
ykuroda | 0:13a5d365ba16 | 442 | check_template_parameters(); |
ykuroda | 0:13a5d365ba16 | 443 | |
ykuroda | 0:13a5d365ba16 | 444 | eigen_assert(a.rows()==a.cols()); |
ykuroda | 0:13a5d365ba16 | 445 | const Index size = a.rows(); |
ykuroda | 0:13a5d365ba16 | 446 | |
ykuroda | 0:13a5d365ba16 | 447 | m_matrix = a; |
ykuroda | 0:13a5d365ba16 | 448 | |
ykuroda | 0:13a5d365ba16 | 449 | m_transpositions.resize(size); |
ykuroda | 0:13a5d365ba16 | 450 | m_isInitialized = false; |
ykuroda | 0:13a5d365ba16 | 451 | m_temporary.resize(size); |
ykuroda | 0:13a5d365ba16 | 452 | m_sign = internal::ZeroSign; |
ykuroda | 0:13a5d365ba16 | 453 | |
ykuroda | 0:13a5d365ba16 | 454 | internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign); |
ykuroda | 0:13a5d365ba16 | 455 | |
ykuroda | 0:13a5d365ba16 | 456 | m_isInitialized = true; |
ykuroda | 0:13a5d365ba16 | 457 | return *this; |
ykuroda | 0:13a5d365ba16 | 458 | } |
ykuroda | 0:13a5d365ba16 | 459 | |
ykuroda | 0:13a5d365ba16 | 460 | /** Update the LDLT decomposition: given A = L D L^T, efficiently compute the decomposition of A + sigma w w^T. |
ykuroda | 0:13a5d365ba16 | 461 | * \param w a vector to be incorporated into the decomposition. |
ykuroda | 0:13a5d365ba16 | 462 | * \param sigma a scalar, +1 for updates and -1 for "downdates," which correspond to removing previously-added column vectors. Optional; default value is +1. |
ykuroda | 0:13a5d365ba16 | 463 | * \sa setZero() |
ykuroda | 0:13a5d365ba16 | 464 | */ |
ykuroda | 0:13a5d365ba16 | 465 | template<typename MatrixType, int _UpLo> |
ykuroda | 0:13a5d365ba16 | 466 | template<typename Derived> |
ykuroda | 0:13a5d365ba16 | 467 | LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Derived>& w, const typename LDLT<MatrixType,_UpLo>::RealScalar& sigma) |
ykuroda | 0:13a5d365ba16 | 468 | { |
ykuroda | 0:13a5d365ba16 | 469 | const Index size = w.rows(); |
ykuroda | 0:13a5d365ba16 | 470 | if (m_isInitialized) |
ykuroda | 0:13a5d365ba16 | 471 | { |
ykuroda | 0:13a5d365ba16 | 472 | eigen_assert(m_matrix.rows()==size); |
ykuroda | 0:13a5d365ba16 | 473 | } |
ykuroda | 0:13a5d365ba16 | 474 | else |
ykuroda | 0:13a5d365ba16 | 475 | { |
ykuroda | 0:13a5d365ba16 | 476 | m_matrix.resize(size,size); |
ykuroda | 0:13a5d365ba16 | 477 | m_matrix.setZero(); |
ykuroda | 0:13a5d365ba16 | 478 | m_transpositions.resize(size); |
ykuroda | 0:13a5d365ba16 | 479 | for (Index i = 0; i < size; i++) |
ykuroda | 0:13a5d365ba16 | 480 | m_transpositions.coeffRef(i) = i; |
ykuroda | 0:13a5d365ba16 | 481 | m_temporary.resize(size); |
ykuroda | 0:13a5d365ba16 | 482 | m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef; |
ykuroda | 0:13a5d365ba16 | 483 | m_isInitialized = true; |
ykuroda | 0:13a5d365ba16 | 484 | } |
ykuroda | 0:13a5d365ba16 | 485 | |
ykuroda | 0:13a5d365ba16 | 486 | internal::ldlt_inplace<UpLo>::update(m_matrix, m_transpositions, m_temporary, w, sigma); |
ykuroda | 0:13a5d365ba16 | 487 | |
ykuroda | 0:13a5d365ba16 | 488 | return *this; |
ykuroda | 0:13a5d365ba16 | 489 | } |
ykuroda | 0:13a5d365ba16 | 490 | |
ykuroda | 0:13a5d365ba16 | 491 | namespace internal { |
ykuroda | 0:13a5d365ba16 | 492 | template<typename _MatrixType, int _UpLo, typename Rhs> |
ykuroda | 0:13a5d365ba16 | 493 | struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs> |
ykuroda | 0:13a5d365ba16 | 494 | : solve_retval_base<LDLT<_MatrixType,_UpLo>, Rhs> |
ykuroda | 0:13a5d365ba16 | 495 | { |
ykuroda | 0:13a5d365ba16 | 496 | typedef LDLT<_MatrixType,_UpLo> LDLTType; |
ykuroda | 0:13a5d365ba16 | 497 | EIGEN_MAKE_SOLVE_HELPERS(LDLTType,Rhs) |
ykuroda | 0:13a5d365ba16 | 498 | |
ykuroda | 0:13a5d365ba16 | 499 | template<typename Dest> void evalTo(Dest& dst) const |
ykuroda | 0:13a5d365ba16 | 500 | { |
ykuroda | 0:13a5d365ba16 | 501 | eigen_assert(rhs().rows() == dec().matrixLDLT().rows()); |
ykuroda | 0:13a5d365ba16 | 502 | // dst = P b |
ykuroda | 0:13a5d365ba16 | 503 | dst = dec().transpositionsP() * rhs(); |
ykuroda | 0:13a5d365ba16 | 504 | |
ykuroda | 0:13a5d365ba16 | 505 | // dst = L^-1 (P b) |
ykuroda | 0:13a5d365ba16 | 506 | dec().matrixL().solveInPlace(dst); |
ykuroda | 0:13a5d365ba16 | 507 | |
ykuroda | 0:13a5d365ba16 | 508 | // dst = D^-1 (L^-1 P b) |
ykuroda | 0:13a5d365ba16 | 509 | // more precisely, use pseudo-inverse of D (see bug 241) |
ykuroda | 0:13a5d365ba16 | 510 | using std::abs; |
ykuroda | 0:13a5d365ba16 | 511 | using std::max; |
ykuroda | 0:13a5d365ba16 | 512 | typedef typename LDLTType::MatrixType MatrixType; |
ykuroda | 0:13a5d365ba16 | 513 | typedef typename LDLTType::RealScalar RealScalar; |
ykuroda | 0:13a5d365ba16 | 514 | const typename Diagonal<const MatrixType>::RealReturnType vectorD(dec().vectorD()); |
ykuroda | 0:13a5d365ba16 | 515 | // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon |
ykuroda | 0:13a5d365ba16 | 516 | // as motivated by LAPACK's xGELSS: |
ykuroda | 0:13a5d365ba16 | 517 | // RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() *NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest()); |
ykuroda | 0:13a5d365ba16 | 518 | // However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest |
ykuroda | 0:13a5d365ba16 | 519 | // diagonal element is not well justified and to numerical issues in some cases. |
ykuroda | 0:13a5d365ba16 | 520 | // Moreover, Lapack's xSYTRS routines use 0 for the tolerance. |
ykuroda | 0:13a5d365ba16 | 521 | RealScalar tolerance = RealScalar(1) / NumTraits<RealScalar>::highest(); |
ykuroda | 0:13a5d365ba16 | 522 | |
ykuroda | 0:13a5d365ba16 | 523 | for (Index i = 0; i < vectorD.size(); ++i) { |
ykuroda | 0:13a5d365ba16 | 524 | if(abs(vectorD(i)) > tolerance) |
ykuroda | 0:13a5d365ba16 | 525 | dst.row(i) /= vectorD(i); |
ykuroda | 0:13a5d365ba16 | 526 | else |
ykuroda | 0:13a5d365ba16 | 527 | dst.row(i).setZero(); |
ykuroda | 0:13a5d365ba16 | 528 | } |
ykuroda | 0:13a5d365ba16 | 529 | |
ykuroda | 0:13a5d365ba16 | 530 | // dst = L^-T (D^-1 L^-1 P b) |
ykuroda | 0:13a5d365ba16 | 531 | dec().matrixU().solveInPlace(dst); |
ykuroda | 0:13a5d365ba16 | 532 | |
ykuroda | 0:13a5d365ba16 | 533 | // dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b |
ykuroda | 0:13a5d365ba16 | 534 | dst = dec().transpositionsP().transpose() * dst; |
ykuroda | 0:13a5d365ba16 | 535 | } |
ykuroda | 0:13a5d365ba16 | 536 | }; |
ykuroda | 0:13a5d365ba16 | 537 | } |
ykuroda | 0:13a5d365ba16 | 538 | |
ykuroda | 0:13a5d365ba16 | 539 | /** \internal use x = ldlt_object.solve(x); |
ykuroda | 0:13a5d365ba16 | 540 | * |
ykuroda | 0:13a5d365ba16 | 541 | * This is the \em in-place version of solve(). |
ykuroda | 0:13a5d365ba16 | 542 | * |
ykuroda | 0:13a5d365ba16 | 543 | * \param bAndX represents both the right-hand side matrix b and result x. |
ykuroda | 0:13a5d365ba16 | 544 | * |
ykuroda | 0:13a5d365ba16 | 545 | * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD. |
ykuroda | 0:13a5d365ba16 | 546 | * |
ykuroda | 0:13a5d365ba16 | 547 | * This version avoids a copy when the right hand side matrix b is not |
ykuroda | 0:13a5d365ba16 | 548 | * needed anymore. |
ykuroda | 0:13a5d365ba16 | 549 | * |
ykuroda | 0:13a5d365ba16 | 550 | * \sa LDLT::solve(), MatrixBase::ldlt() |
ykuroda | 0:13a5d365ba16 | 551 | */ |
ykuroda | 0:13a5d365ba16 | 552 | template<typename MatrixType,int _UpLo> |
ykuroda | 0:13a5d365ba16 | 553 | template<typename Derived> |
ykuroda | 0:13a5d365ba16 | 554 | bool LDLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const |
ykuroda | 0:13a5d365ba16 | 555 | { |
ykuroda | 0:13a5d365ba16 | 556 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 557 | eigen_assert(m_matrix.rows() == bAndX.rows()); |
ykuroda | 0:13a5d365ba16 | 558 | |
ykuroda | 0:13a5d365ba16 | 559 | bAndX = this->solve(bAndX); |
ykuroda | 0:13a5d365ba16 | 560 | |
ykuroda | 0:13a5d365ba16 | 561 | return true; |
ykuroda | 0:13a5d365ba16 | 562 | } |
ykuroda | 0:13a5d365ba16 | 563 | |
ykuroda | 0:13a5d365ba16 | 564 | /** \returns the matrix represented by the decomposition, |
ykuroda | 0:13a5d365ba16 | 565 | * i.e., it returns the product: P^T L D L^* P. |
ykuroda | 0:13a5d365ba16 | 566 | * This function is provided for debug purpose. */ |
ykuroda | 0:13a5d365ba16 | 567 | template<typename MatrixType, int _UpLo> |
ykuroda | 0:13a5d365ba16 | 568 | MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const |
ykuroda | 0:13a5d365ba16 | 569 | { |
ykuroda | 0:13a5d365ba16 | 570 | eigen_assert(m_isInitialized && "LDLT is not initialized."); |
ykuroda | 0:13a5d365ba16 | 571 | const Index size = m_matrix.rows(); |
ykuroda | 0:13a5d365ba16 | 572 | MatrixType res(size,size); |
ykuroda | 0:13a5d365ba16 | 573 | |
ykuroda | 0:13a5d365ba16 | 574 | // P |
ykuroda | 0:13a5d365ba16 | 575 | res.setIdentity(); |
ykuroda | 0:13a5d365ba16 | 576 | res = transpositionsP() * res; |
ykuroda | 0:13a5d365ba16 | 577 | // L^* P |
ykuroda | 0:13a5d365ba16 | 578 | res = matrixU() * res; |
ykuroda | 0:13a5d365ba16 | 579 | // D(L^*P) |
ykuroda | 0:13a5d365ba16 | 580 | res = vectorD().real().asDiagonal() * res; |
ykuroda | 0:13a5d365ba16 | 581 | // L(DL^*P) |
ykuroda | 0:13a5d365ba16 | 582 | res = matrixL() * res; |
ykuroda | 0:13a5d365ba16 | 583 | // P^T (LDL^*P) |
ykuroda | 0:13a5d365ba16 | 584 | res = transpositionsP().transpose() * res; |
ykuroda | 0:13a5d365ba16 | 585 | |
ykuroda | 0:13a5d365ba16 | 586 | return res; |
ykuroda | 0:13a5d365ba16 | 587 | } |
ykuroda | 0:13a5d365ba16 | 588 | |
ykuroda | 0:13a5d365ba16 | 589 | /** \cholesky_module |
ykuroda | 0:13a5d365ba16 | 590 | * \returns the Cholesky decomposition with full pivoting without square root of \c *this |
ykuroda | 0:13a5d365ba16 | 591 | */ |
ykuroda | 0:13a5d365ba16 | 592 | template<typename MatrixType, unsigned int UpLo> |
ykuroda | 0:13a5d365ba16 | 593 | inline const LDLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo> |
ykuroda | 0:13a5d365ba16 | 594 | SelfAdjointView<MatrixType, UpLo>::ldlt() const |
ykuroda | 0:13a5d365ba16 | 595 | { |
ykuroda | 0:13a5d365ba16 | 596 | return LDLT<PlainObject,UpLo>(m_matrix); |
ykuroda | 0:13a5d365ba16 | 597 | } |
ykuroda | 0:13a5d365ba16 | 598 | |
ykuroda | 0:13a5d365ba16 | 599 | /** \cholesky_module |
ykuroda | 0:13a5d365ba16 | 600 | * \returns the Cholesky decomposition with full pivoting without square root of \c *this |
ykuroda | 0:13a5d365ba16 | 601 | */ |
ykuroda | 0:13a5d365ba16 | 602 | template<typename Derived> |
ykuroda | 0:13a5d365ba16 | 603 | inline const LDLT<typename MatrixBase<Derived>::PlainObject> |
ykuroda | 0:13a5d365ba16 | 604 | MatrixBase<Derived>::ldlt() const |
ykuroda | 0:13a5d365ba16 | 605 | { |
ykuroda | 0:13a5d365ba16 | 606 | return LDLT<PlainObject>(derived()); |
ykuroda | 0:13a5d365ba16 | 607 | } |
ykuroda | 0:13a5d365ba16 | 608 | |
ykuroda | 0:13a5d365ba16 | 609 | } // end namespace Eigen |
ykuroda | 0:13a5d365ba16 | 610 | |
ykuroda | 0:13a5d365ba16 | 611 | #endif // EIGEN_LDLT_H |