Teh first one
[mldemos:kalians-mldemos.git] / _AlgorithmsPlugins / KPCA / Eigen / src / Cholesky / LDLT.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2009 Keir Mierle <mierle@gmail.com>
6 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
7 //
8 // Eigen is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 3 of the License, or (at your option) any later version.
12 //
13 // Alternatively, you can redistribute it and/or
14 // modify it under the terms of the GNU General Public License as
15 // published by the Free Software Foundation; either version 2 of
16 // the License, or (at your option) any later version.
17 //
18 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
19 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
20 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
21 // GNU General Public License for more details.
22 //
23 // You should have received a copy of the GNU Lesser General Public
24 // License and a copy of the GNU General Public License along with
25 // Eigen. If not, see <http://www.gnu.org/licenses/>.
26
27 #ifndef EIGEN_LDLT_H
28 #define EIGEN_LDLT_H
29
30 namespace internal {
31 template<typename MatrixType, int UpLo> struct LDLT_Traits;
32 }
33
34 /** \ingroup cholesky_Module
35   *
36   * \class LDLT
37   *
38   * \brief Robust Cholesky decomposition of a matrix with pivoting
39   *
40   * \param MatrixType the type of the matrix of which to compute the LDL^T Cholesky decomposition
41   *
42   * Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite
43   * matrix \f$ A \f$ such that \f$ A =  P^TLDL^*P \f$, where P is a permutation matrix, L
44   * is lower triangular with a unit diagonal and D is a diagonal matrix.
45   *
46   * The decomposition uses pivoting to ensure stability, so that L will have
47   * zeros in the bottom right rank(A) - n submatrix. Avoiding the square root
48   * on D also stabilizes the computation.
49   *
50   * Remember that Cholesky decompositions are not rank-revealing. Also, do not use a Cholesky
51         * decomposition to determine whether a system of equations has a solution.
52   *
53   * \sa MatrixBase::ldlt(), class LLT
54   */
55  /* THIS PART OF THE DOX IS CURRENTLY DISABLED BECAUSE INACCURATE BECAUSE OF BUG IN THE DECOMPOSITION CODE
56   * Note that during the decomposition, only the upper triangular part of A is considered. Therefore,
57   * the strict lower part does not have to store correct values.
58   */
59 template<typename _MatrixType, int _UpLo> class LDLT
60 {
61   public:
62     typedef _MatrixType MatrixType;
63     enum {
64       RowsAtCompileTime = MatrixType::RowsAtCompileTime,
65       ColsAtCompileTime = MatrixType::ColsAtCompileTime,
66       Options = MatrixType::Options & ~RowMajorBit, // these are the options for the TmpMatrixType, we need a ColMajor matrix here!
67       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
68       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
69       UpLo = _UpLo
70     };
71     typedef typename MatrixType::Scalar Scalar;
72     typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
73     typedef typename MatrixType::Index Index;
74     typedef Matrix<Scalar, RowsAtCompileTime, 1, Options, MaxRowsAtCompileTime, 1> TmpMatrixType;
75
76     typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
77     typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType;
78
79     typedef internal::LDLT_Traits<MatrixType,UpLo> Traits;
80
81     /** \brief Default Constructor.
82       *
83       * The default constructor is useful in cases in which the user intends to
84       * perform decompositions via LDLT::compute(const MatrixType&).
85       */
86     LDLT() : m_matrix(), m_transpositions(), m_isInitialized(false) {}
87
88     /** \brief Default Constructor with memory preallocation
89       *
90       * Like the default constructor but with preallocation of the internal data
91       * according to the specified problem \a size.
92       * \sa LDLT()
93       */
94     LDLT(Index size)
95       : m_matrix(size, size),
96         m_transpositions(size),
97         m_temporary(size),
98         m_isInitialized(false)
99     {}
100
101     LDLT(const MatrixType& matrix)
102       : m_matrix(matrix.rows(), matrix.cols()),
103         m_transpositions(matrix.rows()),
104         m_temporary(matrix.rows()),
105         m_isInitialized(false)
106     {
107       compute(matrix);
108     }
109
110     /** \returns a view of the upper triangular matrix U */
111     inline typename Traits::MatrixU matrixU() const
112     {
113       eigen_assert(m_isInitialized && "LDLT is not initialized.");
114       return Traits::getU(m_matrix);
115     }
116
117     /** \returns a view of the lower triangular matrix L */
118     inline typename Traits::MatrixL matrixL() const
119     {
120       eigen_assert(m_isInitialized && "LDLT is not initialized.");
121       return Traits::getL(m_matrix);
122     }
123
124     /** \returns the permutation matrix P as a transposition sequence.
125       */
126     inline const TranspositionType& transpositionsP() const
127     {
128       eigen_assert(m_isInitialized && "LDLT is not initialized.");
129       return m_transpositions;
130     }
131
132     /** \returns the coefficients of the diagonal matrix D */
133     inline Diagonal<const MatrixType> vectorD(void) const
134     {
135       eigen_assert(m_isInitialized && "LDLT is not initialized.");
136       return m_matrix.diagonal();
137     }
138
139     /** \returns true if the matrix is positive (semidefinite) */
140     inline bool isPositive(void) const
141     {
142       eigen_assert(m_isInitialized && "LDLT is not initialized.");
143       return m_sign == 1;
144     }
145     
146     #ifdef EIGEN2_SUPPORT
147     inline bool isPositiveDefinite() const
148     {
149       return isPositive();
150     }
151     #endif
152
153     /** \returns true if the matrix is negative (semidefinite) */
154     inline bool isNegative(void) const
155     {
156       eigen_assert(m_isInitialized && "LDLT is not initialized.");
157       return m_sign == -1;
158     }
159
160     /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
161       *
162       * \note_about_checking_solutions
163       *
164       * \sa solveInPlace(), MatrixBase::ldlt()
165       */
166     template<typename Rhs>
167     inline const internal::solve_retval<LDLT, Rhs>
168     solve(const MatrixBase<Rhs>& b) const
169     {
170       eigen_assert(m_isInitialized && "LDLT is not initialized.");
171       eigen_assert(m_matrix.rows()==b.rows()
172                 && "LDLT::solve(): invalid number of rows of the right hand side matrix b");
173       return internal::solve_retval<LDLT, Rhs>(*this, b.derived());
174     }
175
176     #ifdef EIGEN2_SUPPORT
177     template<typename OtherDerived, typename ResultType>
178     bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const
179     {
180       *result = this->solve(b);
181       return true;
182     }
183     #endif
184
185     template<typename Derived>
186     bool solveInPlace(MatrixBase<Derived> &bAndX) const;
187
188     LDLT& compute(const MatrixType& matrix);
189
190     /** \returns the internal LDLT decomposition matrix
191       *
192       * TODO: document the storage layout
193       */
194     inline const MatrixType& matrixLDLT() const
195     {
196       eigen_assert(m_isInitialized && "LDLT is not initialized.");
197       return m_matrix;
198     }
199
200     MatrixType reconstructedMatrix() const;
201
202     inline Index rows() const { return m_matrix.rows(); }
203     inline Index cols() const { return m_matrix.cols(); }
204
205   protected:
206
207     /** \internal
208       * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U.
209       * The strict upper part is used during the decomposition, the strict lower
210       * part correspond to the coefficients of L (its diagonal is equal to 1 and
211       * is not stored), and the diagonal entries correspond to D.
212       */
213     MatrixType m_matrix;
214     TranspositionType m_transpositions;
215     TmpMatrixType m_temporary;
216     int m_sign;
217     bool m_isInitialized;
218 };
219
220 namespace internal {
221
222 template<int UpLo> struct ldlt_inplace;
223
224 template<> struct ldlt_inplace<Lower>
225 {
226   template<typename MatrixType, typename TranspositionType, typename Workspace>
227   static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
228   {
229     typedef typename MatrixType::Scalar Scalar;
230     typedef typename MatrixType::RealScalar RealScalar;
231     typedef typename MatrixType::Index Index;
232     eigen_assert(mat.rows()==mat.cols());
233     const Index size = mat.rows();
234
235     if (size <= 1)
236     {
237       transpositions.setIdentity();
238       if(sign)
239         *sign = real(mat.coeff(0,0))>0 ? 1:-1;
240       return true;
241     }
242
243     RealScalar cutoff = 0, biggest_in_corner;
244
245     for (Index k = 0; k < size; ++k)
246     {
247       // Find largest diagonal element
248       Index index_of_biggest_in_corner;
249       biggest_in_corner = mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
250       index_of_biggest_in_corner += k;
251
252       if(k == 0)
253       {
254         // The biggest overall is the point of reference to which further diagonals
255         // are compared; if any diagonal is negligible compared
256         // to the largest overall, the algorithm bails.
257         cutoff = abs(NumTraits<Scalar>::epsilon() * biggest_in_corner);
258
259         if(sign)
260           *sign = real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0 ? 1 : -1;
261       }
262
263       // Finish early if the matrix is not full rank.
264       if(biggest_in_corner < cutoff)
265       {
266         for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i;
267         break;
268       }
269
270       transpositions.coeffRef(k) = index_of_biggest_in_corner;
271       if(k != index_of_biggest_in_corner)
272       {
273         // apply the transposition while taking care to consider only
274         // the lower triangular part
275         Index s = size-index_of_biggest_in_corner-1; // trailing size after the biggest element
276         mat.row(k).head(k).swap(mat.row(index_of_biggest_in_corner).head(k));
277         mat.col(k).tail(s).swap(mat.col(index_of_biggest_in_corner).tail(s));
278         std::swap(mat.coeffRef(k,k),mat.coeffRef(index_of_biggest_in_corner,index_of_biggest_in_corner));
279         for(int i=k+1;i<index_of_biggest_in_corner;++i)
280         {
281           Scalar tmp = mat.coeffRef(i,k);
282           mat.coeffRef(i,k) = conj(mat.coeffRef(index_of_biggest_in_corner,i));
283           mat.coeffRef(index_of_biggest_in_corner,i) = conj(tmp);
284         }
285         if(NumTraits<Scalar>::IsComplex)
286           mat.coeffRef(index_of_biggest_in_corner,k) = conj(mat.coeff(index_of_biggest_in_corner,k));
287       }
288
289       // partition the matrix:
290       //       A00 |  -  |  -
291       // lu  = A10 | A11 |  -
292       //       A20 | A21 | A22
293       Index rs = size - k - 1;
294       Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1);
295       Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k);
296       Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k);
297
298       if(k>0)
299       {
300         temp.head(k) = mat.diagonal().head(k).asDiagonal() * A10.adjoint();
301         mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
302         if(rs>0)
303           A21.noalias() -= A20 * temp.head(k);
304       }
305       if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff))
306         A21 /= mat.coeffRef(k,k);
307     }
308
309     return true;
310   }
311 };
312
313 template<> struct ldlt_inplace<Upper>
314 {
315   template<typename MatrixType, typename TranspositionType, typename Workspace>
316   static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
317   {
318     Transpose<MatrixType> matt(mat);
319     return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
320   }
321 };
322
323 template<typename MatrixType> struct LDLT_Traits<MatrixType,Lower>
324 {
325   typedef TriangularView<MatrixType, UnitLower> MatrixL;
326   typedef TriangularView<typename MatrixType::AdjointReturnType, UnitUpper> MatrixU;
327   inline static MatrixL getL(const MatrixType& m) { return m; }
328   inline static MatrixU getU(const MatrixType& m) { return m.adjoint(); }
329 };
330
331 template<typename MatrixType> struct LDLT_Traits<MatrixType,Upper>
332 {
333   typedef TriangularView<typename MatrixType::AdjointReturnType, UnitLower> MatrixL;
334   typedef TriangularView<MatrixType, UnitUpper> MatrixU;
335   inline static MatrixL getL(const MatrixType& m) { return m.adjoint(); }
336   inline static MatrixU getU(const MatrixType& m) { return m; }
337 };
338
339 } // end namespace internal
340
341 /** Compute / recompute the LDLT decomposition A = L D L^* = U^* D U of \a matrix
342   */
343 template<typename MatrixType, int _UpLo>
344 LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
345 {
346   eigen_assert(a.rows()==a.cols());
347   const Index size = a.rows();
348
349   m_matrix = a;
350
351   m_transpositions.resize(size);
352   m_isInitialized = false;
353   m_temporary.resize(size);
354
355   internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign);
356
357   m_isInitialized = true;
358   return *this;
359 }
360
361 namespace internal {
362 template<typename _MatrixType, int _UpLo, typename Rhs>
363 struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
364   : solve_retval_base<LDLT<_MatrixType,_UpLo>, Rhs>
365 {
366   typedef LDLT<_MatrixType,_UpLo> LDLTType;
367   EIGEN_MAKE_SOLVE_HELPERS(LDLTType,Rhs)
368
369   template<typename Dest> void evalTo(Dest& dst) const
370   {
371     eigen_assert(rhs().rows() == dec().matrixLDLT().rows());
372     // dst = P b
373     dst = dec().transpositionsP() * rhs();
374
375     // dst = L^-1 (P b)
376     dec().matrixL().solveInPlace(dst);
377
378     // dst = D^-1 (L^-1 P b)
379     dst = dec().vectorD().asDiagonal().inverse() * dst;
380
381     // dst = L^-T (D^-1 L^-1 P b)
382     dec().matrixU().solveInPlace(dst);
383
384     // dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b
385     dst = dec().transpositionsP().transpose() * dst;
386   }
387 };
388 }
389
390 /** \internal use x = ldlt_object.solve(x);
391   *
392   * This is the \em in-place version of solve().
393   *
394   * \param bAndX represents both the right-hand side matrix b and result x.
395   *
396   * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
397   *
398   * This version avoids a copy when the right hand side matrix b is not
399   * needed anymore.
400   *
401   * \sa LDLT::solve(), MatrixBase::ldlt()
402   */
403 template<typename MatrixType,int _UpLo>
404 template<typename Derived>
405 bool LDLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const
406 {
407   eigen_assert(m_isInitialized && "LDLT is not initialized.");
408   const Index size = m_matrix.rows();
409   eigen_assert(size == bAndX.rows());
410
411   bAndX = this->solve(bAndX);
412
413   return true;
414 }
415
416 /** \returns the matrix represented by the decomposition,
417  * i.e., it returns the product: P^T L D L^* P.
418  * This function is provided for debug purpose. */
419 template<typename MatrixType, int _UpLo>
420 MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
421 {
422   eigen_assert(m_isInitialized && "LDLT is not initialized.");
423   const Index size = m_matrix.rows();
424   MatrixType res(size,size);
425
426   // P
427   res.setIdentity();
428   res = transpositionsP() * res;
429   // L^* P
430   res = matrixU() * res;
431   // D(L^*P)
432   res = vectorD().asDiagonal() * res;
433   // L(DL^*P)
434   res = matrixL() * res;
435   // P^T (LDL^*P)
436   res = transpositionsP().transpose() * res;
437
438   return res;
439 }
440
441 /** \cholesky_module
442   * \returns the Cholesky decomposition with full pivoting without square root of \c *this
443   */
444 template<typename MatrixType, unsigned int UpLo>
445 inline const LDLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo>
446 SelfAdjointView<MatrixType, UpLo>::ldlt() const
447 {
448   return LDLT<PlainObject,UpLo>(m_matrix);
449 }
450
451 /** \cholesky_module
452   * \returns the Cholesky decomposition with full pivoting without square root of \c *this
453   */
454 template<typename Derived>
455 inline const LDLT<typename MatrixBase<Derived>::PlainObject>
456 MatrixBase<Derived>::ldlt() const
457 {
458   return LDLT<PlainObject>(derived());
459 }
460
461 #endif // EIGEN_LDLT_H