IncompleteCholesky.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) 2012 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_INCOMPLETE_CHOlESKY_H
00011 #define EIGEN_INCOMPLETE_CHOlESKY_H
00012 #include "Eigen/src/IterativeLinearSolvers/IncompleteLUT.h" 
00013 #include <Eigen/OrderingMethods>
00014 #include <list>
00015 
00016 namespace Eigen {  
00029 template <typename Scalar, int _UpLo = Lower, typename _OrderingType = NaturalOrdering<int> >
00030 class IncompleteCholesky : internal::noncopyable
00031 {
00032   public:
00033     typedef SparseMatrix<Scalar,ColMajor> MatrixType;
00034     typedef _OrderingType OrderingType;
00035     typedef typename MatrixType::RealScalar RealScalar; 
00036     typedef typename MatrixType::Index Index; 
00037     typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
00038     typedef Matrix<Scalar,Dynamic,1> ScalarType; 
00039     typedef Matrix<Index,Dynamic, 1> IndexType;
00040     typedef std::vector<std::list<Index> > VectorList; 
00041     enum { UpLo = _UpLo };
00042   public:
00043     IncompleteCholesky() : m_shift(1),m_factorizationIsOk(false) {}
00044     IncompleteCholesky(const MatrixType& matrix) : m_shift(1),m_factorizationIsOk(false)
00045     {
00046       compute(matrix);
00047     }
00048     
00049     Index rows() const { return m_L.rows(); }
00050     
00051     Index cols() const { return m_L.cols(); }
00052     
00053 
00059     ComputationInfo info() const
00060     {
00061       eigen_assert(m_isInitialized && "IncompleteLLT is not initialized.");
00062       return m_info;
00063     }
00064     
00068     void setShift( Scalar shift) { m_shift = shift; }
00069     
00073     template<typename MatrixType>
00074     void analyzePattern(const MatrixType& mat)
00075     {
00076       OrderingType ord; 
00077       ord(mat.template selfadjointView<UpLo>(), m_perm); 
00078       m_analysisIsOk = true; 
00079     }
00080     
00081     template<typename MatrixType>
00082     void factorize(const MatrixType& amat);
00083     
00084     template<typename MatrixType>
00085     void compute (const MatrixType& matrix)
00086     {
00087       analyzePattern(matrix); 
00088       factorize(matrix);
00089     }
00090     
00091     template<typename Rhs, typename Dest>
00092     void _solve(const Rhs& b, Dest& x) const
00093     {
00094       eigen_assert(m_factorizationIsOk && "factorize() should be called first");
00095       if (m_perm.rows() == b.rows())
00096         x = m_perm.inverse() * b; 
00097       else 
00098         x = b; 
00099       x = m_scal.asDiagonal() * x;
00100       x = m_L.template triangularView<UnitLower>().solve(x); 
00101       x = m_L.adjoint().template triangularView<Upper>().solve(x); 
00102       if (m_perm.rows() == b.rows())
00103         x = m_perm * x;
00104       x = m_scal.asDiagonal() * x;
00105     }
00106     template<typename Rhs> inline const internal::solve_retval<IncompleteCholesky, Rhs>
00107     solve(const MatrixBase<Rhs>& b) const
00108     {
00109       eigen_assert(m_factorizationIsOk && "IncompleteLLT did not succeed");
00110       eigen_assert(m_isInitialized && "IncompleteLLT is not initialized.");
00111       eigen_assert(cols()==b.rows()
00112                 && "IncompleteLLT::solve(): invalid number of rows of the right hand side matrix b");
00113       return internal::solve_retval<IncompleteCholesky, Rhs>(*this, b.derived());
00114     }
00115   protected:
00116     SparseMatrix<Scalar,ColMajor> m_L;  // The lower part stored in CSC
00117     ScalarType m_scal; // The vector for scaling the matrix 
00118     Scalar m_shift; //The initial shift parameter
00119     bool m_analysisIsOk; 
00120     bool m_factorizationIsOk; 
00121     bool m_isInitialized;
00122     ComputationInfo m_info;
00123     PermutationType m_perm; 
00124     
00125   private:
00126     template <typename IdxType, typename SclType>
00127     inline void updateList(const IdxType& colPtr, IdxType& rowIdx, SclType& vals, const Index& col, const Index& jk, IndexType& firstElt, VectorList& listCol); 
00128 }; 
00129 
00130 template<typename Scalar, int _UpLo, typename OrderingType>
00131 template<typename _MatrixType>
00132 void IncompleteCholesky<Scalar,_UpLo, OrderingType>::factorize(const _MatrixType& mat)
00133 {
00134   using std::sqrt;
00135   using std::min;
00136   eigen_assert(m_analysisIsOk && "analyzePattern() should be called first"); 
00137     
00138   // Dropping strategies : Keep only the p largest elements per column, where p is the number of elements in the column of the original matrix. Other strategies will be added
00139   
00140   // Apply the fill-reducing permutation computed in analyzePattern()
00141   if (m_perm.rows() == mat.rows() ) // To detect the null permutation
00142     m_L.template selfadjointView<Lower>() = mat.template selfadjointView<_UpLo>().twistedBy(m_perm);
00143   else
00144     m_L.template selfadjointView<Lower>() = mat.template selfadjointView<_UpLo>();
00145   
00146   Index n = m_L.cols(); 
00147   Index nnz = m_L.nonZeros();
00148   Map<ScalarType> vals(m_L.valuePtr(), nnz); //values
00149   Map<IndexType> rowIdx(m_L.innerIndexPtr(), nnz);  //Row indices
00150   Map<IndexType> colPtr( m_L.outerIndexPtr(), n+1); // Pointer to the beginning of each row
00151   IndexType firstElt(n-1); // for each j, points to the next entry in vals that will be used in the factorization
00152   VectorList listCol(n); // listCol(j) is a linked list of columns to update column j
00153   ScalarType curCol(n); // Store a  nonzero values in each column
00154   IndexType irow(n); // Row indices of nonzero elements in each column
00155   
00156   
00157   // Computes the scaling factors 
00158   m_scal.resize(n);
00159   for (int j = 0; j < n; j++)
00160   {
00161     m_scal(j) = m_L.col(j).norm();
00162     m_scal(j) = sqrt(m_scal(j));
00163   }
00164   // Scale and compute the shift for the matrix 
00165   Scalar mindiag = vals[0];
00166   for (int j = 0; j < n; j++){
00167     for (int k = colPtr[j]; k < colPtr[j+1]; k++)
00168      vals[k] /= (m_scal(j) * m_scal(rowIdx[k]));
00169     mindiag = (min)(vals[colPtr[j]], mindiag);
00170   }
00171   
00172   if(mindiag < Scalar(0.)) m_shift = m_shift - mindiag;
00173   // Apply the shift to the diagonal elements of the matrix
00174   for (int j = 0; j < n; j++)
00175     vals[colPtr[j]] += m_shift;
00176   // jki version of the Cholesky factorization 
00177   for (int j=0; j < n; ++j)
00178   {  
00179     //Left-looking factorize the column j 
00180     // First, load the jth column into curCol 
00181     Scalar diag = vals[colPtr[j]];  // It is assumed that only the lower part is stored
00182     curCol.setZero();
00183     irow.setLinSpaced(n,0,n-1); 
00184     for (int i = colPtr[j] + 1; i < colPtr[j+1]; i++)
00185     {
00186       curCol(rowIdx[i]) = vals[i]; 
00187       irow(rowIdx[i]) = rowIdx[i]; 
00188     }
00189     std::list<int>::iterator k; 
00190     // Browse all previous columns that will update column j
00191     for(k = listCol[j].begin(); k != listCol[j].end(); k++) 
00192     {
00193       int jk = firstElt(*k); // First element to use in the column 
00194       jk += 1; 
00195       for (int i = jk; i < colPtr[*k+1]; i++)
00196       {
00197         curCol(rowIdx[i]) -= vals[i] * vals[jk] ;
00198       }
00199       updateList(colPtr,rowIdx,vals, *k, jk, firstElt, listCol);
00200     }
00201     
00202     // Scale the current column
00203     if(RealScalar(diag) <= 0) 
00204     {
00205       std::cerr << "\nNegative diagonal during Incomplete factorization... "<< j << "\n";
00206       m_info = NumericalIssue; 
00207       return; 
00208     }
00209     RealScalar rdiag = sqrt(RealScalar(diag));
00210     vals[colPtr[j]] = rdiag;
00211     for (int i = j+1; i < n; i++)
00212     {
00213       //Scale 
00214       curCol(i) /= rdiag;
00215       //Update the remaining diagonals with curCol
00216       vals[colPtr[i]] -= curCol(i) * curCol(i);
00217     }
00218     // Select the largest p elements
00219     //  p is the original number of elements in the column (without the diagonal)
00220     int p = colPtr[j+1] - colPtr[j] - 1 ; 
00221     internal::QuickSplit(curCol, irow, p); 
00222     // Insert the largest p elements in the matrix
00223     int cpt = 0; 
00224     for (int i = colPtr[j]+1; i < colPtr[j+1]; i++)
00225     {
00226       vals[i] = curCol(cpt); 
00227       rowIdx[i] = irow(cpt); 
00228       cpt ++; 
00229     }
00230     // Get the first smallest row index and put it after the diagonal element
00231     Index jk = colPtr(j)+1;
00232     updateList(colPtr,rowIdx,vals,j,jk,firstElt,listCol); 
00233   }
00234   m_factorizationIsOk = true; 
00235   m_isInitialized = true;
00236   m_info = Success; 
00237 }
00238 
00239 template<typename Scalar, int _UpLo, typename OrderingType>
00240 template <typename IdxType, typename SclType>
00241 inline void IncompleteCholesky<Scalar,_UpLo, OrderingType>::updateList(const IdxType& colPtr, IdxType& rowIdx, SclType& vals, const Index& col, const Index& jk, IndexType& firstElt, VectorList& listCol)
00242 {
00243   if (jk < colPtr(col+1) )
00244   {
00245     Index p = colPtr(col+1) - jk;
00246     Index minpos; 
00247     rowIdx.segment(jk,p).minCoeff(&minpos);
00248     minpos += jk;
00249     if (rowIdx(minpos) != rowIdx(jk))
00250     {
00251       //Swap
00252       std::swap(rowIdx(jk),rowIdx(minpos));
00253       std::swap(vals(jk),vals(minpos));
00254     }
00255     firstElt(col) = jk;
00256     listCol[rowIdx(jk)].push_back(col);
00257   }
00258 }
00259 namespace internal {
00260 
00261 template<typename _Scalar, int _UpLo, typename OrderingType, typename Rhs>
00262 struct solve_retval<IncompleteCholesky<_Scalar,  _UpLo, OrderingType>, Rhs>
00263   : solve_retval_base<IncompleteCholesky<_Scalar, _UpLo, OrderingType>, Rhs>
00264 {
00265   typedef IncompleteCholesky<_Scalar, _UpLo, OrderingType> Dec;
00266   EIGEN_MAKE_SOLVE_HELPERS(Dec,Rhs)
00267 
00268   template<typename Dest> void evalTo(Dest& dst) const
00269   {
00270     dec()._solve(rhs(),dst);
00271   }
00272 };
00273 
00274 } // end namespace internal
00275 
00276 } // end namespace Eigen 
00277 
00278 #endif


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