diff --git a/include/albatross/src/eigen/serializable_ldlt.hpp b/include/albatross/src/eigen/serializable_ldlt.hpp index bfafca44..fb7396b9 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 { public: + using RealScalar = double; + using Scalar = double; + using MatrixType = MatrixXd; + SerializableLDLT() : LDLT(){}; SerializableLDLT(const MatrixXd &x) : LDLT(x.ldlt()){}; @@ -43,6 +47,11 @@ class SerializableLDLT : public LDLT { 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 { * Computes the product of the square root of A with rhs, * D^{-1/2} L^-1 P rhs */ - template - Eigen::MatrixXd sqrt_solve(const MatrixBase &rhs) const { + template 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 { diff --git a/include/albatross/src/linalg/block_diagonal.hpp b/include/albatross/src/linalg/block_diagonal.hpp index 0c988de3..2e7ce1eb 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 blocks; template @@ -45,15 +48,27 @@ struct BlockDiagonalLDLT { solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs, ThreadPool *pool) const; + template + inline Eigen::MatrixXd + solve(const Eigen::CwiseBinaryOp &rhs_orig) const; + + template + inline Eigen::MatrixXd + solve(const Eigen::Block &rhs_orig) const; + template Eigen::Matrix<_Scalar, _Rows, _Cols> sqrt_solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs, ThreadPool *pool) const; + const BlockDiagonalLDLT &adjoint() const; + std::map block_to_row_map() const; double log_determinant() const; + double rcond() const; + Eigen::Index rows() const; Eigen::Index cols() const; @@ -126,6 +141,24 @@ BlockDiagonalLDLT::solve( return output; } +template +inline Eigen::MatrixXd +BlockDiagonalLDLT::solve( + const Eigen::CwiseBinaryOp &rhs_orig) const { + ALBATROSS_ASSERT(cols() == rhs_orig.rows()); + Eigen::MatrixXd rhs{rhs_orig}; + return solve(rhs); +} + +template +inline Eigen::MatrixXd BlockDiagonalLDLT::solve( + const Eigen::Block &rhs_orig) + const { + ALBATROSS_ASSERT(cols() == rhs_orig.rows()); + Eigen::MatrixXd rhs{rhs_orig}; + return solve(rhs); +} + template inline Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic> BlockDiagonalLDLT::sqrt_solve( @@ -197,6 +230,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 +257,10 @@ inline Eigen::Index BlockDiagonalLDLT::cols() const { return n; } +inline const BlockDiagonalLDLT &BlockDiagonalLDLT::adjoint() const { + return *this; +} + /* * Block Diagonal */