GCC Code Coverage Report

Directory: ./
File: include/pinocchio/math/lanczos-decomposition.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 0 59 0.0%
Branches: 0 138 0.0%
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