Condition number estimation for custom LDLTs

include/albatross/src/eigen/serializable_ldlt.hpp

@@ -18,6 +18,10 @@ namespace Eigen {
 // See LDLT.h in Eigen for a detailed description of the decomposition
 class SerializableLDLT : public LDLT<MatrixXd, Lower> {
 public:
+  using RealScalar = double;
+  using Scalar = double;
+  using MatrixType = MatrixXd;
+
   SerializableLDLT() : LDLT<MatrixXd, Lower>(){};
 
   SerializableLDLT(const MatrixXd &x) : LDLT<MatrixXd, Lower>(x.ldlt()){};
@@ -43,6 +47,11 @@ class SerializableLDLT : public LDLT<MatrixXd, Lower> {
 
   bool is_initialized() const { return this->m_isInitialized; }
 
+  double l1_norm() const {
+    ALBATROSS_ASSERT(is_initialized() && "Must initialize first!");
+    return this->m_l1_norm;
+  }
+
   /*
    * Computes the inverse of the square root of the diagonal, D^{-1/2}
    */
@@ -88,10 +97,10 @@ class SerializableLDLT : public LDLT<MatrixXd, Lower> {
    * Computes the product of the square root of A with rhs,
    *   D^{-1/2} L^-1 P rhs
    */
-  template <class Rhs>
-  Eigen::MatrixXd sqrt_solve(const MatrixBase<Rhs> &rhs) const {
+  template <class Rhs> Eigen::MatrixXd sqrt_solve(const Rhs &rhs) const {
     return diagonal_sqrt_inverse() *
-           this->matrixL().solve(this->transpositionsP() * rhs);
+           this->matrixL().solve(this->transpositionsP() *
+                                 Eigen::MatrixXd(rhs));
   }
 
   Eigen::MatrixXd sqrt_transpose() const {

include/albatross/src/linalg/block_diagonal.hpp

@@ -22,15 +22,17 @@ struct BlockDiagonalLDLT;
 struct BlockDiagonal;
 
 struct BlockDiagonalLDLT {
+  using RealScalar = Eigen::SerializableLDLT::RealScalar;
+  using Scalar = Eigen::SerializableLDLT::Scalar;
+  using MatrixType = Eigen::SerializableLDLT::MatrixType;
   std::vector<Eigen::SerializableLDLT> blocks;
 
-  template <class _Scalar, int _Rows, int _Cols>
-  Eigen::Matrix<_Scalar, _Rows, _Cols>
-  solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs) const;
+  template <typename Derived>
+  inline Eigen::MatrixXd solve(const Eigen::MatrixBase<Derived> &rhs) const;
 
-  template <class _Scalar, int _Rows, int _Cols>
-  Eigen::Matrix<_Scalar, _Rows, _Cols>
-  sqrt_solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs) const;
+  template <typename Derived>
+  inline Eigen::MatrixXd
+  sqrt_solve(const Eigen::MatrixBase<Derived> &rhs) const;
 
   template <class _Scalar, int _Options, typename _StorageIndex>
   Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic>
@@ -45,15 +47,24 @@ struct BlockDiagonalLDLT {
   solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs,
         ThreadPool *pool) const;
 
+  template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+  inline Eigen::MatrixXd
+  solve(const Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel> &rhs_orig)
+      const;
+
   template <class _Scalar, int _Rows, int _Cols>
   Eigen::Matrix<_Scalar, _Rows, _Cols>
   sqrt_solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs,
              ThreadPool *pool) const;
 
+  const BlockDiagonalLDLT &adjoint() const;
+
   std::map<size_t, Eigen::Index> block_to_row_map() const;
 
   double log_determinant() const;
 
+  double rcond() const;
+
   Eigen::Index rows() const;
 
   Eigen::Index cols() const;
@@ -82,12 +93,12 @@ struct BlockDiagonal {
 /*
  * BlockDiagonalLDLT
  */
-template <class _Scalar, int _Rows, int _Cols>
-inline Eigen::Matrix<_Scalar, _Rows, _Cols> BlockDiagonalLDLT::solve(
-    const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs) const {
+template <typename Derived>
+inline Eigen::MatrixXd
+BlockDiagonalLDLT::solve(const Eigen::MatrixBase<Derived> &rhs) const {
   ALBATROSS_ASSERT(cols() == rhs.rows());
   Eigen::Index i = 0;
-  Eigen::Matrix<_Scalar, _Rows, _Cols> output(rows(), rhs.cols());
+  Eigen::MatrixXd output(rows(), rhs.cols());
   for (const auto &b : blocks) {
     const auto rhs_chunk = rhs.block(i, 0, b.rows(), rhs.cols());
     output.block(i, 0, b.rows(), rhs.cols()) = b.solve(rhs_chunk);
@@ -96,12 +107,12 @@ inline Eigen::Matrix<_Scalar, _Rows, _Cols> BlockDiagonalLDLT::solve(
   return output;
 }
 
-template <class _Scalar, int _Rows, int _Cols>
-inline Eigen::Matrix<_Scalar, _Rows, _Cols> BlockDiagonalLDLT::sqrt_solve(
-    const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs) const {
+template <typename Derived>
+inline Eigen::MatrixXd
+BlockDiagonalLDLT::sqrt_solve(const Eigen::MatrixBase<Derived> &rhs) const {
   ALBATROSS_ASSERT(cols() == rhs.rows());
   Eigen::Index i = 0;
-  Eigen::Matrix<_Scalar, _Rows, _Cols> output(rows(), rhs.cols());
+  Eigen::MatrixXd output(rows(), rhs.cols());
   for (const auto &b : blocks) {
     const auto rhs_chunk = rhs.block(i, 0, b.rows(), rhs.cols());
     output.block(i, 0, b.rows(), rhs.cols()) = b.sqrt_solve(rhs_chunk);
@@ -126,6 +137,15 @@ BlockDiagonalLDLT::solve(
   return output;
 }
 
+template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
+inline Eigen::MatrixXd BlockDiagonalLDLT::solve(
+    const Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel> &rhs_orig)
+    const {
+  ALBATROSS_ASSERT(cols() == rhs_orig.rows());
+  Eigen::MatrixXd rhs{rhs_orig};
+  return solve(rhs);
+}
+
 template <class _Scalar, int _Options, typename _StorageIndex>
 inline Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic>
 BlockDiagonalLDLT::sqrt_solve(
@@ -197,6 +217,17 @@ inline double BlockDiagonalLDLT::log_determinant() const {
   return output;
 }
 
+inline double BlockDiagonalLDLT::rcond() const {
+  // L1 induced norm is just the maximum absolute column sum.
+  // Therefore the L1 induced norm of a block-diagonal matrix is the
+  // maximum of the L1 induced norms of the individual blocks.
+  double l1_norm = -INFINITY;
+  for (const auto &b : blocks) {
+    l1_norm = std::max(l1_norm, b.l1_norm());
+  }
+  return Eigen::internal::rcond_estimate_helper(l1_norm, *this);
+}
+
 inline Eigen::Index BlockDiagonalLDLT::rows() const {
   Eigen::Index n = 0;
   for (const auto &b : blocks) {
@@ -213,6 +244,10 @@ inline Eigen::Index BlockDiagonalLDLT::cols() const {
   return n;
 }
 
+inline const BlockDiagonalLDLT &BlockDiagonalLDLT::adjoint() const {
+  return *this;
+}
+
 /*
  * Block Diagonal
  */