Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2024 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_math_lanczos_decomposition_hpp__ |
6 |
|
|
#define __pinocchio_math_lanczos_decomposition_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/math/fwd.hpp" |
9 |
|
|
#include "pinocchio/math/tridiagonal-matrix.hpp" |
10 |
|
|
#include "pinocchio/math/gram-schmidt-orthonormalisation.hpp" |
11 |
|
|
|
12 |
|
|
namespace pinocchio |
13 |
|
|
{ |
14 |
|
|
|
15 |
|
|
/// \brief Compute the largest eigenvalues and the associated principle eigenvector via power |
16 |
|
|
/// iteration |
17 |
|
|
template<typename _Matrix> |
18 |
|
|
struct LanczosDecompositionTpl |
19 |
|
|
{ |
20 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
21 |
|
|
|
22 |
|
|
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(_Matrix) PlainMatrix; |
23 |
|
|
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename PlainMatrix::ColXpr) Vector; |
24 |
|
|
typedef typename _Matrix::Scalar Scalar; |
25 |
|
|
enum |
26 |
|
|
{ |
27 |
|
|
Options = _Matrix::Options |
28 |
|
|
}; |
29 |
|
|
typedef TridiagonalSymmetricMatrixTpl<Scalar, Options> TridiagonalMatrix; |
30 |
|
|
|
31 |
|
|
/// \brief Default constructor for the Lanczos decomposition from an input matrix |
32 |
|
|
template<typename MatrixLikeType> |
33 |
|
✗ |
LanczosDecompositionTpl(const MatrixLikeType & mat, const Eigen::DenseIndex decomposition_size) |
34 |
|
✗ |
: m_Qs(mat.rows(), decomposition_size) |
35 |
|
✗ |
, m_Ts(decomposition_size) |
36 |
|
✗ |
, m_A_times_q(mat.rows()) |
37 |
|
✗ |
, m_rank(-1) |
38 |
|
|
{ |
39 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT(mat.rows() == mat.cols(), "The input matrix is not square."); |
40 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
41 |
|
|
decomposition_size >= 1, "The size of the decomposition should be greater than one."); |
42 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
43 |
|
|
decomposition_size <= mat.rows(), |
44 |
|
|
"The size of the decomposition should not be larger than the number of rows."); |
45 |
|
|
|
46 |
|
✗ |
compute(mat); |
47 |
|
|
} |
48 |
|
|
|
49 |
|
✗ |
bool operator==(const LanczosDecompositionTpl & other) const |
50 |
|
|
{ |
51 |
|
✗ |
if (this == &other) |
52 |
|
✗ |
return true; |
53 |
|
✗ |
return m_Qs == other.m_Qs && m_Ts == other.m_Ts && m_rank == other.m_rank; |
54 |
|
|
} |
55 |
|
|
|
56 |
|
✗ |
bool operator!=(const LanczosDecompositionTpl & other) const |
57 |
|
|
{ |
58 |
|
✗ |
return !(*this == other); |
59 |
|
|
} |
60 |
|
|
|
61 |
|
|
/// |
62 |
|
|
/// \brief Computes the Lanczos decomposition of the input matrix A |
63 |
|
|
/// |
64 |
|
|
/// \param[in] A The matrix to decompose |
65 |
|
|
/// |
66 |
|
|
template<typename MatrixLikeType> |
67 |
|
✗ |
void compute(const MatrixLikeType & A) |
68 |
|
|
{ |
69 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT(A.rows() == A.cols(), "The input matrix is not square."); |
70 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
71 |
|
|
A.rows() == m_Qs.rows(), "The input matrix is of correct size."); |
72 |
|
|
|
73 |
|
✗ |
const Eigen::DenseIndex decomposition_size = m_Ts.cols(); |
74 |
|
✗ |
auto & alphas = m_Ts.diagonal(); |
75 |
|
✗ |
auto & betas = m_Ts.subDiagonal(); |
76 |
|
|
|
77 |
|
✗ |
m_Qs.col(0).fill(Scalar(1) / math::sqrt(Scalar(m_Qs.rows()))); |
78 |
|
✗ |
m_Ts.setZero(); |
79 |
|
|
Eigen::DenseIndex k; |
80 |
|
✗ |
for (k = 0; k < decomposition_size; ++k) |
81 |
|
|
{ |
82 |
|
✗ |
const auto q = m_Qs.col(k); |
83 |
|
✗ |
m_A_times_q.noalias() = A * q; |
84 |
|
✗ |
alphas[k] = q.dot(m_A_times_q); |
85 |
|
|
|
86 |
|
✗ |
if (k < decomposition_size - 1) |
87 |
|
|
{ |
88 |
|
✗ |
auto q_next = m_Qs.col(k + 1); |
89 |
|
✗ |
m_A_times_q -= alphas[k] * q; |
90 |
|
✗ |
if (k > 0) |
91 |
|
|
{ |
92 |
|
✗ |
m_A_times_q -= betas[k - 1] * m_Qs.col(k - 1); |
93 |
|
|
} |
94 |
|
|
|
95 |
|
|
// Perform Gram-Schmidt orthogonalization procedure. |
96 |
|
✗ |
if (k > 0) |
97 |
|
✗ |
orthonormalisation(m_Qs.leftCols(k), m_A_times_q); |
98 |
|
|
|
99 |
|
|
// Compute beta |
100 |
|
✗ |
betas[k] = m_A_times_q.norm(); |
101 |
|
✗ |
if (betas[k] <= 1e2 * Eigen::NumTraits<Scalar>::epsilon()) |
102 |
|
|
{ |
103 |
|
✗ |
break; |
104 |
|
|
} |
105 |
|
|
else |
106 |
|
|
{ |
107 |
|
✗ |
q_next.noalias() = m_A_times_q / betas[k]; |
108 |
|
|
} |
109 |
|
|
} |
110 |
|
|
} |
111 |
|
✗ |
m_rank = math::max(Eigen::DenseIndex(1), k); |
112 |
|
|
} |
113 |
|
|
|
114 |
|
|
/// |
115 |
|
|
/// \brief Computes the residual associated with the decomposition, namely, the quantity \f$ A |
116 |
|
|
/// Q_s - Q_s T_s \f$ |
117 |
|
|
/// |
118 |
|
|
/// \param[in] A the matrix that have been decomposed. |
119 |
|
|
/// |
120 |
|
|
/// \returns The residual of the decomposition |
121 |
|
|
/// |
122 |
|
|
template<typename MatrixLikeType> |
123 |
|
✗ |
PlainMatrix computeDecompositionResidual(const MatrixLikeType & A) const |
124 |
|
|
{ |
125 |
|
✗ |
const Eigen::DenseIndex last_col_id = m_Ts.cols() - 1; |
126 |
|
✗ |
const auto & alphas = m_Ts.diagonal(); |
127 |
|
✗ |
const auto & betas = m_Ts.subDiagonal(); |
128 |
|
|
|
129 |
|
✗ |
PlainMatrix residual = A * m_Qs; |
130 |
|
✗ |
residual -= (m_Qs * m_Ts).eval(); |
131 |
|
|
|
132 |
|
✗ |
const auto & q = m_Qs.col(last_col_id); |
133 |
|
|
|
134 |
|
✗ |
auto & tmp_vec = m_A_times_q; // use m_A_times_q as a temporary vector |
135 |
|
✗ |
tmp_vec.noalias() = A * q; |
136 |
|
✗ |
tmp_vec -= alphas[last_col_id] * q; |
137 |
|
✗ |
if (last_col_id > 0) |
138 |
|
✗ |
tmp_vec -= betas[last_col_id - 1] * m_Qs.col(last_col_id - 1); |
139 |
|
|
|
140 |
|
✗ |
residual.col(last_col_id) -= tmp_vec; |
141 |
|
|
|
142 |
|
✗ |
return residual; |
143 |
|
|
} |
144 |
|
|
|
145 |
|
|
/// \brief Returns the tridiagonal matrix associated with the Lanczos decomposition |
146 |
|
|
const TridiagonalMatrix & Ts() const |
147 |
|
|
{ |
148 |
|
|
return m_Ts; |
149 |
|
|
} |
150 |
|
|
/// \brief Returns the tridiagonal matrix associated with the Lanczos decomposition |
151 |
|
✗ |
TridiagonalMatrix & Ts() |
152 |
|
|
{ |
153 |
|
✗ |
return m_Ts; |
154 |
|
|
} |
155 |
|
|
|
156 |
|
|
/// \brief Returns the orthogonal basis associated with the Lanczos decomposition |
157 |
|
|
const PlainMatrix & Qs() const |
158 |
|
|
{ |
159 |
|
|
return m_Qs; |
160 |
|
|
} |
161 |
|
|
/// \brief Returns the orthogonal basis associated with the Lanczos decomposition |
162 |
|
✗ |
PlainMatrix & Qs() |
163 |
|
|
{ |
164 |
|
✗ |
return m_Qs; |
165 |
|
|
} |
166 |
|
|
|
167 |
|
|
/// \brief Returns the rank of the decomposition |
168 |
|
✗ |
Eigen::DenseIndex rank() const |
169 |
|
|
{ |
170 |
|
✗ |
return m_rank; |
171 |
|
|
} |
172 |
|
|
|
173 |
|
|
protected: |
174 |
|
|
PlainMatrix m_Qs; |
175 |
|
|
TridiagonalMatrix m_Ts; |
176 |
|
|
mutable Vector m_A_times_q; |
177 |
|
|
Eigen::DenseIndex m_rank; |
178 |
|
|
}; |
179 |
|
|
} // namespace pinocchio |
180 |
|
|
|
181 |
|
|
#endif // ifndef __pinocchio_math_lanczos_decomposition_hpp__ |
182 |
|
|
|