pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
lanczos-decomposition.hpp
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
12namespace pinocchio
13{
14
17 template<typename _Matrix>
19 {
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 };
30
32 template<typename MatrixLikeType>
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
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
122 template<typename MatrixLikeType>
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
141
142 return residual;
143 }
144
146 const TridiagonalMatrix & Ts() const
147 {
148 return m_Ts;
149 }
152 {
153 return m_Ts;
154 }
155
157 const PlainMatrix & Qs() const
158 {
159 return m_Qs;
160 }
163 {
164 return m_Qs;
165 }
166
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__
Main pinocchio namespace.
Definition treeview.dox:11
void orthonormalisation(const Eigen::MatrixBase< MatrixType > &basis, const Eigen::MatrixBase< VectorType > &vec_)
Perform the Gram-Schmidt orthonormalisation on the input/output vector for a given input basis.
Compute the largest eigenvalues and the associated principle eigenvector via power iteration.
const TridiagonalMatrix & Ts() const
Returns the tridiagonal matrix associated with the Lanczos decomposition.
TridiagonalMatrix & Ts()
Returns the tridiagonal matrix associated with the Lanczos decomposition.
PlainMatrix computeDecompositionResidual(const MatrixLikeType &A) const
Computes the residual associated with the decomposition, namely, the quantity .
PlainMatrix & Qs()
Returns the orthogonal basis associated with the Lanczos decomposition.
const PlainMatrix & Qs() const
Returns the orthogonal basis associated with the Lanczos decomposition.
Eigen::DenseIndex rank() const
Returns the rank of the decomposition.
void compute(const MatrixLikeType &A)
Computes the Lanczos decomposition of the input matrix A.
LanczosDecompositionTpl(const MatrixLikeType &mat, const Eigen::DenseIndex decomposition_size)
Default constructor for the Lanczos decomposition from an input matrix.