IncompleteLUT.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_LUT_H
00011 #define EIGEN_INCOMPLETE_LUT_H
00012 
00013 
00014 namespace Eigen { 
00015 
00016 namespace internal {
00017     
00027 template <typename VectorV, typename VectorI, typename Index>
00028 Index QuickSplit(VectorV &row, VectorI &ind, Index ncut)
00029 {
00030   typedef typename VectorV::RealScalar RealScalar;
00031   using std::swap;
00032   using std::abs;
00033   Index mid;
00034   Index n = row.size(); /* length of the vector */
00035   Index first, last ;
00036   
00037   ncut--; /* to fit the zero-based indices */
00038   first = 0; 
00039   last = n-1; 
00040   if (ncut < first || ncut > last ) return 0;
00041   
00042   do {
00043     mid = first; 
00044     RealScalar abskey = abs(row(mid)); 
00045     for (Index j = first + 1; j <= last; j++) {
00046       if ( abs(row(j)) > abskey) {
00047         ++mid;
00048         swap(row(mid), row(j));
00049         swap(ind(mid), ind(j));
00050       }
00051     }
00052     /* Interchange for the pivot element */
00053     swap(row(mid), row(first));
00054     swap(ind(mid), ind(first));
00055     
00056     if (mid > ncut) last = mid - 1;
00057     else if (mid < ncut ) first = mid + 1; 
00058   } while (mid != ncut );
00059   
00060   return 0; /* mid is equal to ncut */ 
00061 }
00062 
00063 }// end namespace internal
00064 
00095 template <typename _Scalar>
00096 class IncompleteLUT : internal::noncopyable
00097 {
00098     typedef _Scalar Scalar;
00099     typedef typename NumTraits<Scalar>::Real RealScalar;
00100     typedef Matrix<Scalar,Dynamic,1> Vector;
00101     typedef SparseMatrix<Scalar,RowMajor> FactorType;
00102     typedef SparseMatrix<Scalar,ColMajor> PermutType;
00103     typedef typename FactorType::Index Index;
00104 
00105   public:
00106     typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
00107     
00108     IncompleteLUT()
00109       : m_droptol(NumTraits<Scalar>::dummy_precision()), m_fillfactor(10),
00110         m_analysisIsOk(false), m_factorizationIsOk(false), m_isInitialized(false)
00111     {}
00112     
00113     template<typename MatrixType>
00114     IncompleteLUT(const MatrixType& mat, const RealScalar& droptol=NumTraits<Scalar>::dummy_precision(), int fillfactor = 10)
00115       : m_droptol(droptol),m_fillfactor(fillfactor),
00116         m_analysisIsOk(false),m_factorizationIsOk(false),m_isInitialized(false)
00117     {
00118       eigen_assert(fillfactor != 0);
00119       compute(mat); 
00120     }
00121     
00122     Index rows() const { return m_lu.rows(); }
00123     
00124     Index cols() const { return m_lu.cols(); }
00125 
00131     ComputationInfo info() const
00132     {
00133       eigen_assert(m_isInitialized && "IncompleteLUT is not initialized.");
00134       return m_info;
00135     }
00136     
00137     template<typename MatrixType>
00138     void analyzePattern(const MatrixType& amat);
00139     
00140     template<typename MatrixType>
00141     void factorize(const MatrixType& amat);
00142     
00148     template<typename MatrixType>
00149     IncompleteLUT<Scalar>& compute(const MatrixType& amat)
00150     {
00151       analyzePattern(amat); 
00152       factorize(amat);
00153       m_isInitialized = m_factorizationIsOk;
00154       return *this;
00155     }
00156 
00157     void setDroptol(const RealScalar& droptol); 
00158     void setFillfactor(int fillfactor); 
00159     
00160     template<typename Rhs, typename Dest>
00161     void _solve(const Rhs& b, Dest& x) const
00162     {
00163       x = m_Pinv * b;  
00164       x = m_lu.template triangularView<UnitLower>().solve(x);
00165       x = m_lu.template triangularView<Upper>().solve(x);
00166       x = m_P * x; 
00167     }
00168 
00169     template<typename Rhs> inline const internal::solve_retval<IncompleteLUT, Rhs>
00170      solve(const MatrixBase<Rhs>& b) const
00171     {
00172       eigen_assert(m_isInitialized && "IncompleteLUT is not initialized.");
00173       eigen_assert(cols()==b.rows()
00174                 && "IncompleteLUT::solve(): invalid number of rows of the right hand side matrix b");
00175       return internal::solve_retval<IncompleteLUT, Rhs>(*this, b.derived());
00176     }
00177 
00178 protected:
00179 
00181     struct keep_diag {
00182       inline bool operator() (const Index& row, const Index& col, const Scalar&) const
00183       {
00184         return row!=col;
00185       }
00186     };
00187 
00188 protected:
00189 
00190     FactorType m_lu;
00191     RealScalar m_droptol;
00192     int m_fillfactor;
00193     bool m_analysisIsOk;
00194     bool m_factorizationIsOk;
00195     bool m_isInitialized;
00196     ComputationInfo m_info;
00197     PermutationMatrix<Dynamic,Dynamic,Index> m_P;     // Fill-reducing permutation
00198     PermutationMatrix<Dynamic,Dynamic,Index> m_Pinv;  // Inverse permutation
00199 };
00200 
00205 template<typename Scalar>
00206 void IncompleteLUT<Scalar>::setDroptol(const RealScalar& droptol)
00207 {
00208   this->m_droptol = droptol;   
00209 }
00210 
00215 template<typename Scalar>
00216 void IncompleteLUT<Scalar>::setFillfactor(int fillfactor)
00217 {
00218   this->m_fillfactor = fillfactor;   
00219 }
00220 
00221 template <typename Scalar>
00222 template<typename _MatrixType>
00223 void IncompleteLUT<Scalar>::analyzePattern(const _MatrixType& amat)
00224 {
00225   // Compute the Fill-reducing permutation
00226   SparseMatrix<Scalar,ColMajor, Index> mat1 = amat;
00227   SparseMatrix<Scalar,ColMajor, Index> mat2 = amat.transpose();
00228   // Symmetrize the pattern
00229   // FIXME for a matrix with nearly symmetric pattern, mat2+mat1 is the appropriate choice.
00230   //       on the other hand for a really non-symmetric pattern, mat2*mat1 should be prefered...
00231   SparseMatrix<Scalar,ColMajor, Index> AtA = mat2 + mat1;
00232   AtA.prune(keep_diag());
00233   internal::minimum_degree_ordering<Scalar, Index>(AtA, m_P);  // Then compute the AMD ordering...
00234 
00235   m_Pinv  = m_P.inverse(); // ... and the inverse permutation
00236 
00237   m_analysisIsOk = true;
00238 }
00239 
00240 template <typename Scalar>
00241 template<typename _MatrixType>
00242 void IncompleteLUT<Scalar>::factorize(const _MatrixType& amat)
00243 {
00244   using std::sqrt;
00245   using std::swap;
00246   using std::abs;
00247 
00248   eigen_assert((amat.rows() == amat.cols()) && "The factorization should be done on a square matrix");
00249   Index n = amat.cols();  // Size of the matrix
00250   m_lu.resize(n,n);
00251   // Declare Working vectors and variables
00252   Vector u(n) ;     // real values of the row -- maximum size is n --
00253   VectorXi ju(n);   // column position of the values in u -- maximum size  is n
00254   VectorXi jr(n);   // Indicate the position of the nonzero elements in the vector u -- A zero location is indicated by -1
00255 
00256   // Apply the fill-reducing permutation
00257   eigen_assert(m_analysisIsOk && "You must first call analyzePattern()");
00258   SparseMatrix<Scalar,RowMajor, Index> mat;
00259   mat = amat.twistedBy(m_Pinv);
00260 
00261   // Initialization
00262   jr.fill(-1);
00263   ju.fill(0);
00264   u.fill(0);
00265 
00266   // number of largest elements to keep in each row:
00267   Index fill_in =   static_cast<Index> (amat.nonZeros()*m_fillfactor)/n+1;
00268   if (fill_in > n) fill_in = n;
00269 
00270   // number of largest nonzero elements to keep in the L and the U part of the current row:
00271   Index nnzL = fill_in/2;
00272   Index nnzU = nnzL;
00273   m_lu.reserve(n * (nnzL + nnzU + 1));
00274 
00275   // global loop over the rows of the sparse matrix
00276   for (Index ii = 0; ii < n; ii++)
00277   {
00278     // 1 - copy the lower and the upper part of the row i of mat in the working vector u
00279 
00280     Index sizeu = 1; // number of nonzero elements in the upper part of the current row
00281     Index sizel = 0; // number of nonzero elements in the lower part of the current row
00282     ju(ii)    = ii;
00283     u(ii)     = 0;
00284     jr(ii)    = ii;
00285     RealScalar rownorm = 0;
00286 
00287     typename FactorType::InnerIterator j_it(mat, ii); // Iterate through the current row ii
00288     for (; j_it; ++j_it)
00289     {
00290       Index k = j_it.index();
00291       if (k < ii)
00292       {
00293         // copy the lower part
00294         ju(sizel) = k;
00295         u(sizel) = j_it.value();
00296         jr(k) = sizel;
00297         ++sizel;
00298       }
00299       else if (k == ii)
00300       {
00301         u(ii) = j_it.value();
00302       }
00303       else
00304       {
00305         // copy the upper part
00306         Index jpos = ii + sizeu;
00307         ju(jpos) = k;
00308         u(jpos) = j_it.value();
00309         jr(k) = jpos;
00310         ++sizeu;
00311       }
00312       rownorm += numext::abs2(j_it.value());
00313     }
00314 
00315     // 2 - detect possible zero row
00316     if(rownorm==0)
00317     {
00318       m_info = NumericalIssue;
00319       return;
00320     }
00321     // Take the 2-norm of the current row as a relative tolerance
00322     rownorm = sqrt(rownorm);
00323 
00324     // 3 - eliminate the previous nonzero rows
00325     Index jj = 0;
00326     Index len = 0;
00327     while (jj < sizel)
00328     {
00329       // In order to eliminate in the correct order,
00330       // we must select first the smallest column index among  ju(jj:sizel)
00331       Index k;
00332       Index minrow = ju.segment(jj,sizel-jj).minCoeff(&k); // k is relative to the segment
00333       k += jj;
00334       if (minrow != ju(jj))
00335       {
00336         // swap the two locations
00337         Index j = ju(jj);
00338         swap(ju(jj), ju(k));
00339         jr(minrow) = jj;   jr(j) = k;
00340         swap(u(jj), u(k));
00341       }
00342       // Reset this location
00343       jr(minrow) = -1;
00344 
00345       // Start elimination
00346       typename FactorType::InnerIterator ki_it(m_lu, minrow);
00347       while (ki_it && ki_it.index() < minrow) ++ki_it;
00348       eigen_internal_assert(ki_it && ki_it.col()==minrow);
00349       Scalar fact = u(jj) / ki_it.value();
00350 
00351       // drop too small elements
00352       if(abs(fact) <= m_droptol)
00353       {
00354         jj++;
00355         continue;
00356       }
00357 
00358       // linear combination of the current row ii and the row minrow
00359       ++ki_it;
00360       for (; ki_it; ++ki_it)
00361       {
00362         Scalar prod = fact * ki_it.value();
00363         Index j       = ki_it.index();
00364         Index jpos    = jr(j);
00365         if (jpos == -1) // fill-in element
00366         {
00367           Index newpos;
00368           if (j >= ii) // dealing with the upper part
00369           {
00370             newpos = ii + sizeu;
00371             sizeu++;
00372             eigen_internal_assert(sizeu<=n);
00373           }
00374           else // dealing with the lower part
00375           {
00376             newpos = sizel;
00377             sizel++;
00378             eigen_internal_assert(sizel<=ii);
00379           }
00380           ju(newpos) = j;
00381           u(newpos) = -prod;
00382           jr(j) = newpos;
00383         }
00384         else
00385           u(jpos) -= prod;
00386       }
00387       // store the pivot element
00388       u(len) = fact;
00389       ju(len) = minrow;
00390       ++len;
00391 
00392       jj++;
00393     } // end of the elimination on the row ii
00394 
00395     // reset the upper part of the pointer jr to zero
00396     for(Index k = 0; k <sizeu; k++) jr(ju(ii+k)) = -1;
00397 
00398     // 4 - partially sort and insert the elements in the m_lu matrix
00399 
00400     // sort the L-part of the row
00401     sizel = len;
00402     len = (std::min)(sizel, nnzL);
00403     typename Vector::SegmentReturnType ul(u.segment(0, sizel));
00404     typename VectorXi::SegmentReturnType jul(ju.segment(0, sizel));
00405     internal::QuickSplit(ul, jul, len);
00406 
00407     // store the largest m_fill elements of the L part
00408     m_lu.startVec(ii);
00409     for(Index k = 0; k < len; k++)
00410       m_lu.insertBackByOuterInnerUnordered(ii,ju(k)) = u(k);
00411 
00412     // store the diagonal element
00413     // apply a shifting rule to avoid zero pivots (we are doing an incomplete factorization)
00414     if (u(ii) == Scalar(0))
00415       u(ii) = sqrt(m_droptol) * rownorm;
00416     m_lu.insertBackByOuterInnerUnordered(ii, ii) = u(ii);
00417 
00418     // sort the U-part of the row
00419     // apply the dropping rule first
00420     len = 0;
00421     for(Index k = 1; k < sizeu; k++)
00422     {
00423       if(abs(u(ii+k)) > m_droptol * rownorm )
00424       {
00425         ++len;
00426         u(ii + len)  = u(ii + k);
00427         ju(ii + len) = ju(ii + k);
00428       }
00429     }
00430     sizeu = len + 1; // +1 to take into account the diagonal element
00431     len = (std::min)(sizeu, nnzU);
00432     typename Vector::SegmentReturnType uu(u.segment(ii+1, sizeu-1));
00433     typename VectorXi::SegmentReturnType juu(ju.segment(ii+1, sizeu-1));
00434     internal::QuickSplit(uu, juu, len);
00435 
00436     // store the largest elements of the U part
00437     for(Index k = ii + 1; k < ii + len; k++)
00438       m_lu.insertBackByOuterInnerUnordered(ii,ju(k)) = u(k);
00439   }
00440 
00441   m_lu.finalize();
00442   m_lu.makeCompressed();
00443 
00444   m_factorizationIsOk = true;
00445   m_info = Success;
00446 }
00447 
00448 namespace internal {
00449 
00450 template<typename _MatrixType, typename Rhs>
00451 struct solve_retval<IncompleteLUT<_MatrixType>, Rhs>
00452   : solve_retval_base<IncompleteLUT<_MatrixType>, Rhs>
00453 {
00454   typedef IncompleteLUT<_MatrixType> Dec;
00455   EIGEN_MAKE_SOLVE_HELPERS(Dec,Rhs)
00456 
00457   template<typename Dest> void evalTo(Dest& dst) const
00458   {
00459     dec()._solve(rhs(),dst);
00460   }
00461 };
00462 
00463 } // end namespace internal
00464 
00465 } // end namespace Eigen
00466 
00467 #endif // EIGEN_INCOMPLETE_LUT_H


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