Skip to content

Commit 877be3a

Browse files
peddiepeddie
authored
Minor enhancements to custom LDLT implementations (#489)
This change is in support of PIC.
1 parent 7e27b1f commit 877be3a

File tree

2 files changed

+101
-17
lines changed

2 files changed

+101
-17
lines changed

‎include/albatross/src/eigen/serializable_ldlt.hpp‎

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,10 @@ namespace Eigen {
1818
// See LDLT.h in Eigen for a detailed description of the decomposition
1919
class SerializableLDLT : public LDLT<MatrixXd, Lower> {
2020
public:
21+
using RealScalar = double;
22+
using Scalar = double;
23+
using MatrixType = MatrixXd;
24+
2125
SerializableLDLT() : LDLT<MatrixXd, Lower>(){};
2226

2327
SerializableLDLT(const MatrixXd &x) : LDLT<MatrixXd, Lower>(x.ldlt()){};
@@ -43,6 +47,11 @@ class SerializableLDLT : public LDLT<MatrixXd, Lower> {
4347

4448
bool is_initialized() const { return this->m_isInitialized; }
4549

50+
double l1_norm() const {
51+
ALBATROSS_ASSERT(is_initialized() && "Must initialize first!");
52+
return this->m_l1_norm;
53+
}
54+
4655
/*
4756
* Computes the inverse of the square root of the diagonal, D^{-1/2}
4857
*/
@@ -88,10 +97,10 @@ class SerializableLDLT : public LDLT<MatrixXd, Lower> {
8897
* Computes the product of the square root of A with rhs,
8998
* D^{-1/2} L^-1 P rhs
9099
*/
91-
template <class Rhs>
92-
Eigen::MatrixXd sqrt_solve(const MatrixBase<Rhs> &rhs) const {
100+
template <class Rhs> Eigen::MatrixXd sqrt_solve(const Rhs &rhs) const {
93101
return diagonal_sqrt_inverse() *
94-
this->matrixL().solve(this->transpositionsP() * rhs);
102+
this->matrixL().solve(this->transpositionsP() *
103+
Eigen::MatrixXd(rhs));
95104
}
96105

97106
Eigen::MatrixXd sqrt_transpose() const {

‎include/albatross/src/linalg/block_diagonal.hpp‎

Lines changed: 89 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -22,30 +22,49 @@ struct BlockDiagonalLDLT;
2222
struct BlockDiagonal;
2323

2424
struct BlockDiagonalLDLT {
25+
using RealScalar = Eigen::SerializableLDLT::RealScalar;
26+
using Scalar = Eigen::SerializableLDLT::Scalar;
27+
using MatrixType = Eigen::SerializableLDLT::MatrixType;
2528
std::vector<Eigen::SerializableLDLT> blocks;
2629

27-
template <class _Scalar, int _Rows, int _Cols>
28-
Eigen::Matrix<_Scalar, _Rows, _Cols>
29-
solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs) const;
30+
template <typename Derived>
31+
inline Eigen::MatrixXd solve(const Eigen::MatrixBase<Derived> &rhs) const;
3032

31-
template <class _Scalar, int _Rows, int _Cols>
32-
Eigen::Matrix<_Scalar, _Rows, _Cols>
33-
sqrt_solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs) const;
33+
template <typename Derived>
34+
inline Eigen::MatrixXd
35+
sqrt_solve(const Eigen::MatrixBase<Derived> &rhs) const;
36+
37+
template <class _Scalar, int _Options, typename _StorageIndex>
38+
Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic>
39+
solve(const Eigen::SparseMatrix<_Scalar, _Options, _StorageIndex> &rhs) const;
40+
41+
template <class _Scalar, int _Options, typename _StorageIndex>
42+
Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic> sqrt_solve(
43+
const Eigen::SparseMatrix<_Scalar, _Options, _StorageIndex> &rhs) const;
3444

3545
template <class _Scalar, int _Rows, int _Cols>
3646
Eigen::Matrix<_Scalar, _Rows, _Cols>
3747
solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs,
3848
ThreadPool *pool) const;
3949

50+
template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
51+
inline Eigen::MatrixXd
52+
solve(const Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel> &rhs_orig)
53+
const;
54+
4055
template <class _Scalar, int _Rows, int _Cols>
4156
Eigen::Matrix<_Scalar, _Rows, _Cols>
4257
sqrt_solve(const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs,
4358
ThreadPool *pool) const;
4459

60+
const BlockDiagonalLDLT &adjoint() const;
61+
4562
std::map<size_t, Eigen::Index> block_to_row_map() const;
4663

4764
double log_determinant() const;
4865

66+
double rcond() const;
67+
4968
Eigen::Index rows() const;
5069

5170
Eigen::Index cols() const;
@@ -74,12 +93,12 @@ struct BlockDiagonal {
7493
/*
7594
* BlockDiagonalLDLT
7695
*/
77-
template <class _Scalar, int _Rows, int _Cols>
78-
inline Eigen::Matrix<_Scalar, _Rows, _Cols> BlockDiagonalLDLT::solve(
79-
const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs) const {
96+
template <typename Derived>
97+
inline Eigen::MatrixXd
98+
BlockDiagonalLDLT::solve(const Eigen::MatrixBase<Derived> &rhs) const {
8099
ALBATROSS_ASSERT(cols() == rhs.rows());
81100
Eigen::Index i = 0;
82-
Eigen::Matrix<_Scalar, _Rows, _Cols> output(rows(), rhs.cols());
101+
Eigen::MatrixXd output(rows(), rhs.cols());
83102
for (const auto &b : blocks) {
84103
const auto rhs_chunk = rhs.block(i, 0, b.rows(), rhs.cols());
85104
output.block(i, 0, b.rows(), rhs.cols()) = b.solve(rhs_chunk);
@@ -88,12 +107,53 @@ inline Eigen::Matrix<_Scalar, _Rows, _Cols> BlockDiagonalLDLT::solve(
88107
return output;
89108
}
90109

91-
template <class _Scalar, int _Rows, int _Cols>
92-
inline Eigen::Matrix<_Scalar, _Rows, _Cols> BlockDiagonalLDLT::sqrt_solve(
93-
const Eigen::Matrix<_Scalar, _Rows, _Cols> &rhs) const {
110+
template <typename Derived>
111+
inline Eigen::MatrixXd
112+
BlockDiagonalLDLT::sqrt_solve(const Eigen::MatrixBase<Derived> &rhs) const {
94113
ALBATROSS_ASSERT(cols() == rhs.rows());
95114
Eigen::Index i = 0;
96-
Eigen::Matrix<_Scalar, _Rows, _Cols> output(rows(), rhs.cols());
115+
Eigen::MatrixXd output(rows(), rhs.cols());
116+
for (const auto &b : blocks) {
117+
const auto rhs_chunk = rhs.block(i, 0, b.rows(), rhs.cols());
118+
output.block(i, 0, b.rows(), rhs.cols()) = b.sqrt_solve(rhs_chunk);
119+
i += b.rows();
120+
}
121+
return output;
122+
}
123+
124+
template <class _Scalar, int _Options, typename _StorageIndex>
125+
inline Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic>
126+
BlockDiagonalLDLT::solve(
127+
const Eigen::SparseMatrix<_Scalar, _Options, _StorageIndex> &rhs) const {
128+
ALBATROSS_ASSERT(cols() == rhs.rows());
129+
Eigen::Index i = 0;
130+
Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic> output(rows(),
131+
rhs.cols());
132+
for (const auto &b : blocks) {
133+
const auto rhs_chunk = rhs.block(i, 0, b.rows(), rhs.cols());
134+
output.block(i, 0, b.rows(), rhs.cols()) = b.solve(rhs_chunk);
135+
i += b.rows();
136+
}
137+
return output;
138+
}
139+
140+
template <typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
141+
inline Eigen::MatrixXd BlockDiagonalLDLT::solve(
142+
const Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel> &rhs_orig)
143+
const {
144+
ALBATROSS_ASSERT(cols() == rhs_orig.rows());
145+
Eigen::MatrixXd rhs{rhs_orig};
146+
return solve(rhs);
147+
}
148+
149+
template <class _Scalar, int _Options, typename _StorageIndex>
150+
inline Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic>
151+
BlockDiagonalLDLT::sqrt_solve(
152+
const Eigen::SparseMatrix<_Scalar, _Options, _StorageIndex> &rhs) const {
153+
ALBATROSS_ASSERT(cols() == rhs.rows());
154+
Eigen::Index i = 0;
155+
Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic> output(rows(),
156+
rhs.cols());
97157
for (const auto &b : blocks) {
98158
const auto rhs_chunk = rhs.block(i, 0, b.rows(), rhs.cols());
99159
output.block(i, 0, b.rows(), rhs.cols()) = b.sqrt_solve(rhs_chunk);
@@ -157,6 +217,17 @@ inline double BlockDiagonalLDLT::log_determinant() const {
157217
return output;
158218
}
159219

220+
inline double BlockDiagonalLDLT::rcond() const {
221+
// L1 induced norm is just the maximum absolute column sum.
222+
// Therefore the L1 induced norm of a block-diagonal matrix is the
223+
// maximum of the L1 induced norms of the individual blocks.
224+
double l1_norm = -INFINITY;
225+
for (const auto &b : blocks) {
226+
l1_norm = std::max(l1_norm, b.l1_norm());
227+
}
228+
return Eigen::internal::rcond_estimate_helper(l1_norm, *this);
229+
}
230+
160231
inline Eigen::Index BlockDiagonalLDLT::rows() const {
161232
Eigen::Index n = 0;
162233
for (const auto &b : blocks) {
@@ -173,6 +244,10 @@ inline Eigen::Index BlockDiagonalLDLT::cols() const {
173244
return n;
174245
}
175246

247+
inline const BlockDiagonalLDLT &BlockDiagonalLDLT::adjoint() const {
248+
return *this;
249+
}
250+
176251
/*
177252
* Block Diagonal
178253
*/

0 commit comments

Comments
 (0)