Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
PartialPivLU.h
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com> 00005 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr> 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_PARTIALLU_H 00012 #define EIGEN_PARTIALLU_H 00013 00014 namespace Eigen { 00015 00016 /** \ingroup LU_Module 00017 * 00018 * \class PartialPivLU 00019 * 00020 * \brief LU decomposition of a matrix with partial pivoting, and related features 00021 * 00022 * \param MatrixType the type of the matrix of which we are computing the LU decomposition 00023 * 00024 * This class represents a LU decomposition of a \b square \b invertible matrix, with partial pivoting: the matrix A 00025 * is decomposed as A = PLU where L is unit-lower-triangular, U is upper-triangular, and P 00026 * is a permutation matrix. 00027 * 00028 * Typically, partial pivoting LU decomposition is only considered numerically stable for square invertible 00029 * matrices. Thus LAPACK's dgesv and dgesvx require the matrix to be square and invertible. The present class 00030 * does the same. It will assert that the matrix is square, but it won't (actually it can't) check that the 00031 * matrix is invertible: it is your task to check that you only use this decomposition on invertible matrices. 00032 * 00033 * The guaranteed safe alternative, working for all matrices, is the full pivoting LU decomposition, provided 00034 * by class FullPivLU. 00035 * 00036 * This is \b not a rank-revealing LU decomposition. Many features are intentionally absent from this class, 00037 * such as rank computation. If you need these features, use class FullPivLU. 00038 * 00039 * This LU decomposition is suitable to invert invertible matrices. It is what MatrixBase::inverse() uses 00040 * in the general case. 00041 * On the other hand, it is \b not suitable to determine whether a given matrix is invertible. 00042 * 00043 * The data of the LU decomposition can be directly accessed through the methods matrixLU(), permutationP(). 00044 * 00045 * \sa MatrixBase::partialPivLu(), MatrixBase::determinant(), MatrixBase::inverse(), MatrixBase::computeInverse(), class FullPivLU 00046 */ 00047 template<typename _MatrixType> class PartialPivLU 00048 { 00049 public: 00050 00051 typedef _MatrixType MatrixType; 00052 enum { 00053 RowsAtCompileTime = MatrixType::RowsAtCompileTime, 00054 ColsAtCompileTime = MatrixType::ColsAtCompileTime, 00055 Options = MatrixType::Options, 00056 MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, 00057 MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime 00058 }; 00059 typedef typename MatrixType::Scalar Scalar; 00060 typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar; 00061 typedef typename internal::traits<MatrixType>::StorageKind StorageKind; 00062 typedef typename MatrixType::Index Index; 00063 typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType ; 00064 typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType ; 00065 00066 00067 /** 00068 * \brief Default Constructor. 00069 * 00070 * The default constructor is useful in cases in which the user intends to 00071 * perform decompositions via PartialPivLU::compute(const MatrixType&). 00072 */ 00073 PartialPivLU(); 00074 00075 /** \brief Default Constructor with memory preallocation 00076 * 00077 * Like the default constructor but with preallocation of the internal data 00078 * according to the specified problem \a size. 00079 * \sa PartialPivLU() 00080 */ 00081 PartialPivLU(Index size); 00082 00083 /** Constructor. 00084 * 00085 * \param matrix the matrix of which to compute the LU decomposition. 00086 * 00087 * \warning The matrix should have full rank (e.g. if it's square, it should be invertible). 00088 * If you need to deal with non-full rank, use class FullPivLU instead. 00089 */ 00090 PartialPivLU(const MatrixType& matrix); 00091 00092 PartialPivLU& compute(const MatrixType& matrix); 00093 00094 /** \returns the LU decomposition matrix: the upper-triangular part is U, the 00095 * unit-lower-triangular part is L (at least for square matrices; in the non-square 00096 * case, special care is needed, see the documentation of class FullPivLU). 00097 * 00098 * \sa matrixL(), matrixU() 00099 */ 00100 inline const MatrixType& matrixLU () const 00101 { 00102 eigen_assert(m_isInitialized && "PartialPivLU is not initialized."); 00103 return m_lu; 00104 } 00105 00106 /** \returns the permutation matrix P. 00107 */ 00108 inline const PermutationType & permutationP () const 00109 { 00110 eigen_assert(m_isInitialized && "PartialPivLU is not initialized."); 00111 return m_p; 00112 } 00113 00114 /** This method returns the solution x to the equation Ax=b, where A is the matrix of which 00115 * *this is the LU decomposition. 00116 * 00117 * \param b the right-hand-side of the equation to solve. Can be a vector or a matrix, 00118 * the only requirement in order for the equation to make sense is that 00119 * b.rows()==A.rows(), where A is the matrix of which *this is the LU decomposition. 00120 * 00121 * \returns the solution. 00122 * 00123 * Example: \include PartialPivLU_solve.cpp 00124 * Output: \verbinclude PartialPivLU_solve.out 00125 * 00126 * Since this PartialPivLU class assumes anyway that the matrix A is invertible, the solution 00127 * theoretically exists and is unique regardless of b. 00128 * 00129 * \sa TriangularView::solve(), inverse(), computeInverse() 00130 */ 00131 template<typename Rhs> 00132 inline const internal::solve_retval<PartialPivLU, Rhs> 00133 solve(const MatrixBase<Rhs>& b) const 00134 { 00135 eigen_assert(m_isInitialized && "PartialPivLU is not initialized."); 00136 return internal::solve_retval<PartialPivLU, Rhs>(*this, b.derived()); 00137 } 00138 00139 /** \returns the inverse of the matrix of which *this is the LU decomposition. 00140 * 00141 * \warning The matrix being decomposed here is assumed to be invertible. If you need to check for 00142 * invertibility, use class FullPivLU instead. 00143 * 00144 * \sa MatrixBase::inverse(), LU::inverse() 00145 */ 00146 inline const internal::solve_retval<PartialPivLU,typename MatrixType::IdentityReturnType> inverse () const 00147 { 00148 eigen_assert(m_isInitialized && "PartialPivLU is not initialized."); 00149 return internal::solve_retval<PartialPivLU,typename MatrixType::IdentityReturnType> 00150 (*this, MatrixType::Identity(m_lu.rows(), m_lu.cols())); 00151 } 00152 00153 /** \returns the determinant of the matrix of which 00154 * *this is the LU decomposition. It has only linear complexity 00155 * (that is, O(n) where n is the dimension of the square matrix) 00156 * as the LU decomposition has already been computed. 00157 * 00158 * \note For fixed-size matrices of size up to 4, MatrixBase::determinant() offers 00159 * optimized paths. 00160 * 00161 * \warning a determinant can be very big or small, so for matrices 00162 * of large enough dimension, there is a risk of overflow/underflow. 00163 * 00164 * \sa MatrixBase::determinant() 00165 */ 00166 typename internal::traits<MatrixType>::Scalar determinant () const; 00167 00168 MatrixType reconstructedMatrix () const; 00169 00170 inline Index rows() const { return m_lu.rows(); } 00171 inline Index cols() const { return m_lu.cols(); } 00172 00173 protected: 00174 00175 static void check_template_parameters() 00176 { 00177 EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar); 00178 } 00179 00180 MatrixType m_lu; 00181 PermutationType m_p; 00182 TranspositionType m_rowsTranspositions; 00183 Index m_det_p; 00184 bool m_isInitialized; 00185 }; 00186 00187 template<typename MatrixType> 00188 PartialPivLU<MatrixType>::PartialPivLU() 00189 : m_lu(), 00190 m_p(), 00191 m_rowsTranspositions(), 00192 m_det_p(0), 00193 m_isInitialized(false) 00194 { 00195 } 00196 00197 template<typename MatrixType> 00198 PartialPivLU<MatrixType>::PartialPivLU(Index size) 00199 : m_lu(size, size), 00200 m_p(size), 00201 m_rowsTranspositions(size), 00202 m_det_p(0), 00203 m_isInitialized(false) 00204 { 00205 } 00206 00207 template<typename MatrixType> 00208 PartialPivLU<MatrixType>::PartialPivLU(const MatrixType& matrix) 00209 : m_lu(matrix.rows(), matrix.rows()), 00210 m_p(matrix.rows()), 00211 m_rowsTranspositions(matrix.rows()), 00212 m_det_p(0), 00213 m_isInitialized(false) 00214 { 00215 compute(matrix); 00216 } 00217 00218 namespace internal { 00219 00220 /** \internal This is the blocked version of fullpivlu_unblocked() */ 00221 template<typename Scalar, int StorageOrder, typename PivIndex> 00222 struct partial_lu_impl 00223 { 00224 // FIXME add a stride to Map, so that the following mapping becomes easier, 00225 // another option would be to create an expression being able to automatically 00226 // warp any Map, Matrix, and Block expressions as a unique type, but since that's exactly 00227 // a Map + stride, why not adding a stride to Map, and convenient ctors from a Matrix, 00228 // and Block. 00229 typedef Map<Matrix<Scalar, Dynamic, Dynamic, StorageOrder> > MapLU; 00230 typedef Block<MapLU, Dynamic, Dynamic> MatrixType; 00231 typedef Block<MatrixType,Dynamic,Dynamic> BlockType; 00232 typedef typename MatrixType::RealScalar RealScalar; 00233 typedef typename MatrixType::Index Index; 00234 00235 /** \internal performs the LU decomposition in-place of the matrix \a lu 00236 * using an unblocked algorithm. 00237 * 00238 * In addition, this function returns the row transpositions in the 00239 * vector \a row_transpositions which must have a size equal to the number 00240 * of columns of the matrix \a lu, and an integer \a nb_transpositions 00241 * which returns the actual number of transpositions. 00242 * 00243 * \returns The index of the first pivot which is exactly zero if any, or a negative number otherwise. 00244 */ 00245 static Index unblocked_lu(MatrixType& lu, PivIndex* row_transpositions, PivIndex& nb_transpositions) 00246 { 00247 const Index rows = lu.rows(); 00248 const Index cols = lu.cols(); 00249 const Index size = (std::min)(rows,cols); 00250 nb_transpositions = 0; 00251 Index first_zero_pivot = -1; 00252 for(Index k = 0; k < size; ++k) 00253 { 00254 Index rrows = rows-k-1; 00255 Index rcols = cols-k-1; 00256 00257 Index row_of_biggest_in_col; 00258 RealScalar biggest_in_corner 00259 = lu.col(k).tail(rows-k).cwiseAbs().maxCoeff(&row_of_biggest_in_col); 00260 row_of_biggest_in_col += k; 00261 00262 row_transpositions[k] = PivIndex(row_of_biggest_in_col); 00263 00264 if(biggest_in_corner != RealScalar(0)) 00265 { 00266 if(k != row_of_biggest_in_col) 00267 { 00268 lu.row(k).swap(lu.row(row_of_biggest_in_col)); 00269 ++nb_transpositions; 00270 } 00271 00272 // FIXME shall we introduce a safe quotient expression in cas 1/lu.coeff(k,k) 00273 // overflow but not the actual quotient? 00274 lu.col(k).tail(rrows) /= lu.coeff(k,k); 00275 } 00276 else if(first_zero_pivot==-1) 00277 { 00278 // the pivot is exactly zero, we record the index of the first pivot which is exactly 0, 00279 // and continue the factorization such we still have A = PLU 00280 first_zero_pivot = k; 00281 } 00282 00283 if(k<rows-1) 00284 lu.bottomRightCorner(rrows,rcols).noalias() -= lu.col(k).tail(rrows) * lu.row(k).tail(rcols); 00285 } 00286 return first_zero_pivot; 00287 } 00288 00289 /** \internal performs the LU decomposition in-place of the matrix represented 00290 * by the variables \a rows, \a cols, \a lu_data, and \a lu_stride using a 00291 * recursive, blocked algorithm. 00292 * 00293 * In addition, this function returns the row transpositions in the 00294 * vector \a row_transpositions which must have a size equal to the number 00295 * of columns of the matrix \a lu, and an integer \a nb_transpositions 00296 * which returns the actual number of transpositions. 00297 * 00298 * \returns The index of the first pivot which is exactly zero if any, or a negative number otherwise. 00299 * 00300 * \note This very low level interface using pointers, etc. is to: 00301 * 1 - reduce the number of instanciations to the strict minimum 00302 * 2 - avoid infinite recursion of the instanciations with Block<Block<Block<...> > > 00303 */ 00304 static Index blocked_lu(Index rows, Index cols, Scalar* lu_data, Index luStride, PivIndex* row_transpositions, PivIndex& nb_transpositions, Index maxBlockSize=256) 00305 { 00306 MapLU lu1(lu_data,StorageOrder==RowMajor?rows:luStride,StorageOrder==RowMajor?luStride:cols); 00307 MatrixType lu(lu1,0,0,rows,cols); 00308 00309 const Index size = (std::min)(rows,cols); 00310 00311 // if the matrix is too small, no blocking: 00312 if(size<=16) 00313 { 00314 return unblocked_lu(lu, row_transpositions, nb_transpositions); 00315 } 00316 00317 // automatically adjust the number of subdivisions to the size 00318 // of the matrix so that there is enough sub blocks: 00319 Index blockSize; 00320 { 00321 blockSize = size/8; 00322 blockSize = (blockSize/16)*16; 00323 blockSize = (std::min)((std::max)(blockSize,Index(8)), maxBlockSize); 00324 } 00325 00326 nb_transpositions = 0; 00327 Index first_zero_pivot = -1; 00328 for(Index k = 0; k < size; k+=blockSize) 00329 { 00330 Index bs = (std::min)(size-k,blockSize); // actual size of the block 00331 Index trows = rows - k - bs; // trailing rows 00332 Index tsize = size - k - bs; // trailing size 00333 00334 // partition the matrix: 00335 // A00 | A01 | A02 00336 // lu = A_0 | A_1 | A_2 = A10 | A11 | A12 00337 // A20 | A21 | A22 00338 BlockType A_0(lu,0,0,rows,k); 00339 BlockType A_2(lu,0,k+bs,rows,tsize); 00340 BlockType A11(lu,k,k,bs,bs); 00341 BlockType A12(lu,k,k+bs,bs,tsize); 00342 BlockType A21(lu,k+bs,k,trows,bs); 00343 BlockType A22(lu,k+bs,k+bs,trows,tsize); 00344 00345 PivIndex nb_transpositions_in_panel; 00346 // recursively call the blocked LU algorithm on [A11^T A21^T]^T 00347 // with a very small blocking size: 00348 Index ret = blocked_lu(trows+bs, bs, &lu.coeffRef(k,k), luStride, 00349 row_transpositions+k, nb_transpositions_in_panel, 16); 00350 if(ret>=0 && first_zero_pivot==-1) 00351 first_zero_pivot = k+ret; 00352 00353 nb_transpositions += nb_transpositions_in_panel; 00354 // update permutations and apply them to A_0 00355 for(Index i=k; i<k+bs; ++i) 00356 { 00357 Index piv = (row_transpositions[i] += k); 00358 A_0.row(i).swap(A_0.row(piv)); 00359 } 00360 00361 if(trows) 00362 { 00363 // apply permutations to A_2 00364 for(Index i=k;i<k+bs; ++i) 00365 A_2.row(i).swap(A_2.row(row_transpositions[i])); 00366 00367 // A12 = A11^-1 A12 00368 A11.template triangularView<UnitLower>().solveInPlace(A12); 00369 00370 A22.noalias() -= A21 * A12; 00371 } 00372 } 00373 return first_zero_pivot; 00374 } 00375 }; 00376 00377 /** \internal performs the LU decomposition with partial pivoting in-place. 00378 */ 00379 template<typename MatrixType, typename TranspositionType> 00380 void partial_lu_inplace(MatrixType& lu, TranspositionType& row_transpositions, typename TranspositionType::Index& nb_transpositions) 00381 { 00382 eigen_assert(lu.cols() == row_transpositions.size()); 00383 eigen_assert((&row_transpositions.coeffRef(1)-&row_transpositions.coeffRef(0)) == 1); 00384 00385 partial_lu_impl 00386 <typename MatrixType::Scalar, MatrixType::Flags&RowMajorBit?RowMajor:ColMajor, typename TranspositionType::Index> 00387 ::blocked_lu(lu.rows(), lu.cols(), &lu.coeffRef(0,0), lu.outerStride(), &row_transpositions.coeffRef(0), nb_transpositions); 00388 } 00389 00390 } // end namespace internal 00391 00392 template<typename MatrixType> 00393 PartialPivLU<MatrixType>& PartialPivLU<MatrixType>::compute(const MatrixType& matrix) 00394 { 00395 check_template_parameters(); 00396 00397 // the row permutation is stored as int indices, so just to be sure: 00398 eigen_assert(matrix.rows()<NumTraits<int>::highest()); 00399 00400 m_lu = matrix; 00401 00402 eigen_assert(matrix.rows() == matrix.cols() && "PartialPivLU is only for square (and moreover invertible) matrices"); 00403 const Index size = matrix.rows(); 00404 00405 m_rowsTranspositions.resize(size); 00406 00407 typename TranspositionType::Index nb_transpositions; 00408 internal::partial_lu_inplace(m_lu, m_rowsTranspositions, nb_transpositions); 00409 m_det_p = (nb_transpositions%2) ? -1 : 1; 00410 00411 m_p = m_rowsTranspositions; 00412 00413 m_isInitialized = true; 00414 return *this; 00415 } 00416 00417 template<typename MatrixType> 00418 typename internal::traits<MatrixType>::Scalar PartialPivLU<MatrixType>::determinant () const 00419 { 00420 eigen_assert(m_isInitialized && "PartialPivLU is not initialized."); 00421 return Scalar(m_det_p) * m_lu.diagonal().prod(); 00422 } 00423 00424 /** \returns the matrix represented by the decomposition, 00425 * i.e., it returns the product: P^{-1} L U. 00426 * This function is provided for debug purpose. */ 00427 template<typename MatrixType> 00428 MatrixType PartialPivLU<MatrixType>::reconstructedMatrix () const 00429 { 00430 eigen_assert(m_isInitialized && "LU is not initialized."); 00431 // LU 00432 MatrixType res = m_lu.template triangularView<UnitLower>().toDenseMatrix() 00433 * m_lu.template triangularView<Upper>(); 00434 00435 // P^{-1}(LU) 00436 res = m_p.inverse() * res; 00437 00438 return res; 00439 } 00440 00441 /***** Implementation of solve() *****************************************************/ 00442 00443 namespace internal { 00444 00445 template<typename _MatrixType, typename Rhs> 00446 struct solve_retval<PartialPivLU<_MatrixType>, Rhs> 00447 : solve_retval_base<PartialPivLU<_MatrixType>, Rhs> 00448 { 00449 EIGEN_MAKE_SOLVE_HELPERS(PartialPivLU<_MatrixType>,Rhs) 00450 00451 template<typename Dest> void evalTo(Dest& dst) const 00452 { 00453 /* The decomposition PA = LU can be rewritten as A = P^{-1} L U. 00454 * So we proceed as follows: 00455 * Step 1: compute c = Pb. 00456 * Step 2: replace c by the solution x to Lx = c. 00457 * Step 3: replace c by the solution x to Ux = c. 00458 */ 00459 00460 eigen_assert(rhs().rows() == dec().matrixLU().rows()); 00461 00462 // Step 1 00463 dst = dec().permutationP() * rhs(); 00464 00465 // Step 2 00466 dec().matrixLU().template triangularView<UnitLower>().solveInPlace(dst); 00467 00468 // Step 3 00469 dec().matrixLU().template triangularView<Upper>().solveInPlace(dst); 00470 } 00471 }; 00472 00473 } // end namespace internal 00474 00475 /******** MatrixBase methods *******/ 00476 00477 /** \lu_module 00478 * 00479 * \return the partial-pivoting LU decomposition of \c *this. 00480 * 00481 * \sa class PartialPivLU 00482 */ 00483 template<typename Derived> 00484 inline const PartialPivLU<typename MatrixBase<Derived>::PlainObject> 00485 MatrixBase<Derived>::partialPivLu () const 00486 { 00487 return PartialPivLU<PlainObject>(eval()); 00488 } 00489 00490 #if EIGEN2_SUPPORT_STAGE > STAGE20_RESOLVE_API_CONFLICTS 00491 /** \lu_module 00492 * 00493 * Synonym of partialPivLu(). 00494 * 00495 * \return the partial-pivoting LU decomposition of \c *this. 00496 * 00497 * \sa class PartialPivLU 00498 */ 00499 template<typename Derived> 00500 inline const PartialPivLU<typename MatrixBase<Derived>::PlainObject> 00501 MatrixBase<Derived>::lu () const 00502 { 00503 return PartialPivLU<PlainObject>(eval()); 00504 } 00505 #endif 00506 00507 } // end namespace Eigen 00508 00509 #endif // EIGEN_PARTIALLU_H
Generated on Tue Jul 12 2022 17:46:58 by 1.7.2