From cfed1d5adf93ce0cd62d8a79f4c5015c9afb777a Mon Sep 17 00:00:00 2001
From: Matt Peddie <matt.peddie@swift-nav.com>
Date: Wed, 5 Jun 2024 16:36:45 +1000
Subject: [PATCH] Condition number estimation for custom LDLTs

---
 .../albatross/src/eigen/serializable_ldlt.hpp |  9 ++++++++
 .../albatross/src/linalg/block_diagonal.hpp   | 22 +++++++++++++++++++
 2 files changed, 31 insertions(+)

diff --git a/include/albatross/src/eigen/serializable_ldlt.hpp b/include/albatross/src/eigen/serializable_ldlt.hpp
index bfafca44..8ece4a43 100644
--- a/include/albatross/src/eigen/serializable_ldlt.hpp
+++ b/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}
    */
diff --git a/include/albatross/src/linalg/block_diagonal.hpp b/include/albatross/src/linalg/block_diagonal.hpp
index 0c988de3..3bb4b1de 100644
--- a/include/albatross/src/linalg/block_diagonal.hpp
+++ b/include/albatross/src/linalg/block_diagonal.hpp
@@ -22,6 +22,9 @@ 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>
@@ -50,10 +53,14 @@ struct BlockDiagonalLDLT {
   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;
@@ -197,6 +204,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 +231,10 @@ inline Eigen::Index BlockDiagonalLDLT::cols() const {
   return n;
 }
 
+inline const BlockDiagonalLDLT &BlockDiagonalLDLT::adjoint() const {
+  return *this;
+}
+
 /*
  * Block Diagonal
  */