LDLT: make it honors the Lower/Upper directive and make it works inplace

5 files changed, 160 insertions, 87 deletions

diff --git a/Eigen/src/Cholesky/LDLT.h b/Eigen/src/Cholesky/LDLT.h
index 81f3aaa32..60cb98307 100644
--- a/Eigen/src/Cholesky/LDLT.h
+++ b/Eigen/src/Cholesky/LDLT.h
@@ -27,6 +27,8 @@
 #ifndef EIGEN_LDLT_H
 #define EIGEN_LDLT_H
 
+template<typename MatrixType, int UpLo> struct LDLT_Traits;
+
 /** \ingroup cholesky_Module
   *
   * \class LDLT
@@ -52,7 +54,7 @@
   * Note that during the decomposition, only the upper triangular part of A is considered. Therefore,
   * the strict lower part does not have to store correct values.
   */
-template<typename _MatrixType> class LDLT
+template<typename _MatrixType, int _UpLo> class LDLT
 {
   public:
     typedef _MatrixType MatrixType;
@@ -61,7 +63,8 @@ template<typename _MatrixType> class LDLT
       ColsAtCompileTime = MatrixType::ColsAtCompileTime,
       Options = MatrixType::Options,
       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
+      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
+      UpLo = _UpLo
     };
     typedef typename MatrixType::Scalar Scalar;
     typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
@@ -69,6 +72,8 @@ template<typename _MatrixType> class LDLT
     typedef typename ei_plain_col_type<MatrixType, Index>::type IntColVectorType;
     typedef Matrix<Scalar, RowsAtCompileTime, 1, Options, MaxRowsAtCompileTime, 1> TmpMatrixType;
 
+    typedef LDLT_Traits<MatrixType,UpLo> Traits;
+
     /** \brief Default Constructor.
       *
       * The default constructor is useful in cases in which the user intends to
@@ -100,11 +105,18 @@ template<typename _MatrixType> class LDLT
       compute(matrix);
     }
 
-    /** \returns an expression of the lower triangular matrix L */
-    inline TriangularView<MatrixType, UnitLower> matrixL(void) const
+    /** \returns a view of the upper triangular matrix U */
+    inline typename Traits::MatrixU matrixU() const
     {
       ei_assert(m_isInitialized && "LDLT is not initialized.");
-      return m_matrix;
+      return Traits::getU(m_matrix);
+    }
+
+    /** \returns a view of the lower triangular matrix L */
+    inline typename Traits::MatrixL matrixL() const
+    {
+      ei_assert(m_isInitialized && "LDLT is not initialized.");
+      return Traits::getL(m_matrix);
     }
 
     /** \returns a vector of integers, whose size is the number of rows of the matrix being decomposed,
@@ -186,14 +198,115 @@ template<typename _MatrixType> class LDLT
     IntColVectorType m_p;
     IntColVectorType m_transpositions; // FIXME do we really need to store permanently the transpositions?
     TmpMatrixType m_temporary;
-    Index m_sign;
+    int m_sign;
     bool m_isInitialized;
 };
 
+template<int UpLo> struct ei_ldlt_inplace;
+
+template<> struct ei_ldlt_inplace<Lower>
+{
+  template<typename MatrixType, typename Transpositions, typename Workspace>
+  static bool unblocked(MatrixType& mat, Transpositions& transpositions, Workspace& temp, int* sign=0)
+  {
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef typename MatrixType::Index Index;
+    ei_assert(mat.rows()==mat.cols());
+    const Index size = mat.rows();
+    
+    if (size <= 1)
+    {
+      transpositions.setZero();
+      if(sign)
+        *sign = ei_real(mat.coeff(0,0))>0 ? 1:-1;
+      return true;
+    }
+
+    RealScalar cutoff = 0, biggest_in_corner;
+
+    for (Index k = 0; k < size; ++k)
+    {
+      // Find largest diagonal element
+      Index index_of_biggest_in_corner;
+      biggest_in_corner = mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
+      index_of_biggest_in_corner += k;
+
+      if(k == 0)
+      {
+        // The biggest overall is the point of reference to which further diagonals
+        // are compared; if any diagonal is negligible compared
+        // to the largest overall, the algorithm bails.
+        cutoff = ei_abs(NumTraits<Scalar>::epsilon() * biggest_in_corner);
+
+        if(sign)
+          *sign = ei_real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0 ? 1 : -1;
+      }
+
+      // Finish early if the matrix is not full rank.
+      if(biggest_in_corner < cutoff)
+      {
+        for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i;
+        break;
+      }
+
+      transpositions.coeffRef(k) = index_of_biggest_in_corner;
+      if(k != index_of_biggest_in_corner)
+      {
+        mat.row(k).swap(mat.row(index_of_biggest_in_corner));
+        mat.col(k).swap(mat.col(index_of_biggest_in_corner));
+      }
+
+      Index rs = size - k - 1;
+      Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1);
+      Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k);
+      Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k);
+
+      if(k>0)
+      {
+        temp.head(k) = mat.diagonal().head(k).asDiagonal() * A10.adjoint();
+        mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
+        if(rs>0)
+          A21.noalias() -= A20 * temp.head(k);
+      }
+      if((rs>0) && (ei_abs(mat.coeffRef(k,k)) > cutoff))
+        A21 /= mat.coeffRef(k,k);
+    }
+    
+    return true;
+  }
+};
+
+template<> struct ei_ldlt_inplace<Upper>
+{
+  template<typename MatrixType, typename Transpositions, typename Workspace>
+  static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, Transpositions& transpositions, Workspace& temp, int* sign=0)
+  {
+    Transpose<MatrixType> matt(mat);
+    return ei_ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
+  }
+};
+
+template<typename MatrixType> struct LDLT_Traits<MatrixType,Lower>
+{
+  typedef TriangularView<MatrixType, UnitLower> MatrixL;
+  typedef TriangularView<typename MatrixType::AdjointReturnType, UnitUpper> MatrixU;
+  inline static MatrixL getL(const MatrixType& m) { return m; }
+  inline static MatrixU getU(const MatrixType& m) { return m.adjoint(); }
+};
+
+template<typename MatrixType> struct LDLT_Traits<MatrixType,Upper>
+{
+  typedef TriangularView<typename MatrixType::AdjointReturnType, UnitLower> MatrixL;
+  typedef TriangularView<MatrixType, UnitUpper> MatrixU;
+  inline static MatrixL getL(const MatrixType& m) { return m.adjoint(); }
+  inline static MatrixU getU(const MatrixType& m) { return m; }
+};
+
 /** Compute / recompute the LDLT decomposition A = L D L^* = U^* D U of \a matrix
   */
-template<typename MatrixType>
-LDLT<MatrixType>& LDLT<MatrixType>::compute(const MatrixType& a)
+template<typename MatrixType, int _UpLo>
+LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
 {
   ei_assert(a.rows()==a.cols());
   const Index size = a.rows();
@@ -203,85 +316,29 @@ LDLT<MatrixType>& LDLT<MatrixType>::compute(const MatrixType& a)
   m_p.resize(size);
   m_transpositions.resize(size);
   m_isInitialized = false;
-
-  if (size <= 1)
-  {
-    m_p.setZero();
-    m_transpositions.setZero();
-    m_sign = ei_real(a.coeff(0,0))>0 ? 1:-1;
-    m_isInitialized = true;
-    return *this;
-  }
-
-  RealScalar cutoff = 0, biggest_in_corner;
-
-  // By using a temporary, packet-aligned products are guarenteed. In the LLT
-  // case this is unnecessary because the diagonal is included and will always
-  // have optimal alignment.
   m_temporary.resize(size);
-  for (Index j = 0; j < size; ++j)
-  {
-
-    // Find largest diagonal element
-    Index index_of_biggest_in_corner;
-    biggest_in_corner = m_matrix.diagonal().tail(size-j).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
-    index_of_biggest_in_corner += j;
-
-    if(j == 0)
-    {
-      // The biggest overall is the point of reference to which further diagonals
-      // are compared; if any diagonal is negligible compared
-      // to the largest overall, the algorithm bails.
-      cutoff = ei_abs(NumTraits<Scalar>::epsilon() * biggest_in_corner);
-
-      m_sign = ei_real(m_matrix.diagonal().coeff(index_of_biggest_in_corner)) > 0 ? 1 : -1;
-    }
-
-    // Finish early if the matrix is not full rank.
-    if(biggest_in_corner < cutoff)
-    {
-      for(Index i = j; i < size; i++) m_transpositions.coeffRef(i) = i;
-      break;
-    }
-
-    m_transpositions.coeffRef(j) = index_of_biggest_in_corner;
-    if(j != index_of_biggest_in_corner)
-    {
-      m_matrix.row(j).swap(m_matrix.row(index_of_biggest_in_corner));
-      m_matrix.col(j).swap(m_matrix.col(index_of_biggest_in_corner));
-    }
 
-    Index rs = size - j - 1;
-    Block<MatrixType,Dynamic,1> A21(m_matrix,j+1,j,rs,1);
-    Block<MatrixType,1,Dynamic> A10(m_matrix,j,0,1,j);
-    Block<MatrixType,Dynamic,Dynamic> A20(m_matrix,j+1,0,rs,j);
+  ei_ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign);
 
-    if(j>0)
-    {
-      m_temporary.head(j) = m_matrix.diagonal().head(j).asDiagonal() * A10.adjoint();
-      m_matrix.coeffRef(j,j) -= (A10 * m_temporary.head(j)).value();
-      if(rs>0)
-        A21.noalias() -= A20 * m_temporary.head(j);
-    }
-    if((rs>0) && (ei_abs(m_matrix.coeffRef(j,j)) > cutoff))
-      A21 /= m_matrix.coeffRef(j,j);
-  }
+  if(size==0)
+    m_p.setZero();
 
   // Reverse applied swaps to get P matrix.
   for(Index k = 0; k < size; ++k) m_p.coeffRef(k) = k;
   for(Index k = size-1; k >= 0; --k) {
     std::swap(m_p.coeffRef(k), m_p.coeffRef(m_transpositions.coeff(k)));
   }
-
+    
   m_isInitialized = true;
   return *this;
 }
 
-template<typename _MatrixType, typename Rhs>
-struct ei_solve_retval<LDLT<_MatrixType>, Rhs>
-  : ei_solve_retval_base<LDLT<_MatrixType>, Rhs>
+template<typename _MatrixType, int _UpLo, typename Rhs>
+struct ei_solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
+  : ei_solve_retval_base<LDLT<_MatrixType,_UpLo>, Rhs>
 {
-  EIGEN_MAKE_SOLVE_HELPERS(LDLT<_MatrixType>,Rhs)
+  typedef LDLT<_MatrixType,_UpLo> LDLTType;
+  EIGEN_MAKE_SOLVE_HELPERS(LDLTType,Rhs)
 
   template<typename Dest> void evalTo(Dest& dst) const
   {
@@ -301,9 +358,9 @@ struct ei_solve_retval<LDLT<_MatrixType>, Rhs>
   *
   * \sa LDLT::solve(), MatrixBase::ldlt()
   */
-template<typename MatrixType>
+template<typename MatrixType,int _UpLo>
 template<typename Derived>
-bool LDLT<MatrixType>::solveInPlace(MatrixBase<Derived> &bAndX) const
+bool LDLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const
 {
   ei_assert(m_isInitialized && "LDLT is not initialized.");
   const Index size = m_matrix.rows();
@@ -314,13 +371,13 @@ bool LDLT<MatrixType>::solveInPlace(MatrixBase<Derived> &bAndX) const
 
   // y = L^-1 z
   //matrixL().solveInPlace(bAndX);
-  m_matrix.template triangularView<UnitLower>().solveInPlace(bAndX);
+  matrixL().solveInPlace(bAndX);
 
   // w = D^-1 y
   bAndX = m_matrix.diagonal().asDiagonal().inverse() * bAndX;
 
   // u = L^-T w
-  m_matrix.adjoint().template triangularView<UnitUpper>().solveInPlace(bAndX);
+  matrixU().solveInPlace(bAndX);
 
   // x = P^T u
   for (Index i = size-1; i >= 0; --i) bAndX.row(m_transpositions.coeff(i)).swap(bAndX.row(i));
@@ -331,8 +388,8 @@ bool LDLT<MatrixType>::solveInPlace(MatrixBase<Derived> &bAndX) const
 /** \returns the matrix represented by the decomposition,
  * i.e., it returns the product: P^T L D L^* P.
  * This function is provided for debug purpose. */
-template<typename MatrixType>
-MatrixType LDLT<MatrixType>::reconstructedMatrix() const
+template<typename MatrixType, int _UpLo>
+MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
 {
   ei_assert(m_isInitialized && "LDLT is not initialized.");
   const Index size = m_matrix.rows();
@@ -342,7 +399,7 @@ MatrixType LDLT<MatrixType>::reconstructedMatrix() const
   // PI
   for(Index i = 0; i < size; ++i) res.row(m_transpositions.coeff(i)).swap(res.row(i));
   // L^* P
-  res = matrixL().adjoint() * res;
+  res = matrixU() * res;
   // D(L^*P)
   res = vectorD().asDiagonal() * res;
   // L(DL^*P)
@@ -356,6 +413,16 @@ MatrixType LDLT<MatrixType>::reconstructedMatrix() const
 /** \cholesky_module
   * \returns the Cholesky decomposition with full pivoting without square root of \c *this
   */
+template<typename MatrixType, unsigned int UpLo>
+inline const LDLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo>
+SelfAdjointView<MatrixType, UpLo>::ldlt() const
+{
+  return LDLT<PlainObject,UpLo>(m_matrix);
+}
+
+/** \cholesky_module
+  * \returns the Cholesky decomposition with full pivoting without square root of \c *this
+  */
 template<typename Derived>
 inline const LDLT<typename MatrixBase<Derived>::PlainObject>
 MatrixBase<Derived>::ldlt() const
diff --git a/Eigen/src/Cholesky/LLT.h b/Eigen/src/Cholesky/LLT.h
index 29fa465e1..6e853436c 100644
--- a/Eigen/src/Cholesky/LLT.h
+++ b/Eigen/src/Cholesky/LLT.h
@@ -162,7 +162,6 @@ template<typename _MatrixType, int _UpLo> class LLT
     bool m_isInitialized;
 };
 
-// forward declaration (defined at the end of this file)
 template<int UpLo> struct ei_llt_inplace;
 
 template<> struct ei_llt_inplace<Lower>
diff --git a/Eigen/src/Core/SelfAdjointView.h b/Eigen/src/Core/SelfAdjointView.h
index eed3f9336..84c4dc521 100644
--- a/Eigen/src/Core/SelfAdjointView.h
+++ b/Eigen/src/Core/SelfAdjointView.h
@@ -153,7 +153,7 @@ template<typename MatrixType, unsigned int UpLo> class SelfAdjointView
 /////////// Cholesky module ///////////
 
     const LLT<PlainObject, UpLo> llt() const;
-    const LDLT<PlainObject> ldlt() const;
+    const LDLT<PlainObject, UpLo> ldlt() const;
 
 /////////// Eigenvalue module ///////////
 
diff --git a/Eigen/src/Core/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h
index b3bc9c161..5cf62f4c6 100644
--- a/Eigen/src/Core/util/ForwardDeclarations.h
+++ b/Eigen/src/Core/util/ForwardDeclarations.h
@@ -158,7 +158,7 @@ template<typename MatrixType> class FullPivHouseholderQR;
 template<typename MatrixType> class SVD;
 template<typename MatrixType, unsigned int Options = 0> class JacobiSVD;
 template<typename MatrixType, int UpLo = Lower> class LLT;
-template<typename MatrixType> class LDLT;
+template<typename MatrixType, int UpLo = Lower> class LDLT;
 template<typename VectorsType, typename CoeffsType, int Side=OnTheLeft> class HouseholderSequence;
 template<typename Scalar>     class PlanarRotation;
 
diff --git a/test/cholesky.cpp b/test/cholesky.cpp
index 0ae26c7d5..d403af7ba 100644
--- a/test/cholesky.cpp
+++ b/test/cholesky.cpp
@@ -118,11 +118,18 @@ template<typename MatrixType> void cholesky(const MatrixType& m)
   }
 
   {
-    LDLT<SquareMatrixType> ldlt(symm);
-    VERIFY_IS_APPROX(symm, ldlt.reconstructedMatrix());
-    vecX = ldlt.solve(vecB);
+    LDLT<SquareMatrixType,Lower> ldltlo(symm);
+    VERIFY_IS_APPROX(symm, ldltlo.reconstructedMatrix());
+    vecX = ldltlo.solve(vecB);
     VERIFY_IS_APPROX(symm * vecX, vecB);
-    matX = ldlt.solve(matB);
+    matX = ldltlo.solve(matB);
+    VERIFY_IS_APPROX(symm * matX, matB);
+
+    LDLT<SquareMatrixType,Upper> ldltup(symm);
+    VERIFY_IS_APPROX(symm, ldltup.reconstructedMatrix());
+    vecX = ldltup.solve(vecB);
+    VERIFY_IS_APPROX(symm * vecX, vecB);
+    matX = ldltup.solve(matB);
     VERIFY_IS_APPROX(symm * matX, matB);
   }