5#ifndef __pinocchio_math_lanczos_decomposition_hpp__
6#define __pinocchio_math_lanczos_decomposition_hpp__
8#include "pinocchio/math/fwd.hpp"
9#include "pinocchio/math/tridiagonal-matrix.hpp"
10#include "pinocchio/math/gram-schmidt-orthonormalisation.hpp"
17 template<
typename _Matrix>
23 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(
typename PlainMatrix::ColXpr) Vector;
24 typedef typename _Matrix::Scalar Scalar;
27 Options = _Matrix::Options
32 template<
typename MatrixLikeType>
36 , m_A_times_q(
mat.rows())
39 PINOCCHIO_CHECK_INPUT_ARGUMENT(
mat.rows() ==
mat.cols(),
"The input matrix is not square.");
40 PINOCCHIO_CHECK_INPUT_ARGUMENT(
42 PINOCCHIO_CHECK_INPUT_ARGUMENT(
44 "The size of the decomposition should not be larger than the number of rows.");
53 return m_Qs ==
other.m_Qs && m_Ts ==
other.m_Ts && m_rank ==
other.m_rank;
56 bool operator!=(
const LanczosDecompositionTpl & other)
const
58 return !(*
this == other);
66 template<
typename MatrixLikeType>
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.");
74 auto &
alphas = m_Ts.diagonal();
75 auto &
betas = m_Ts.subDiagonal();
77 m_Qs.col(0).fill(Scalar(1) / math::sqrt(Scalar(m_Qs.rows())));
82 const auto q = m_Qs.col(
k);
83 m_A_times_q.noalias() = A * q;
92 m_A_times_q -=
betas[
k - 1] * m_Qs.col(
k - 1);
100 betas[
k] = m_A_times_q.norm();
101 if (
betas[
k] <= 1
e2 * Eigen::NumTraits<Scalar>::epsilon())
111 m_rank = math::max(Eigen::DenseIndex(1),
k);
122 template<
typename MatrixLikeType>
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();
175 TridiagonalMatrix m_Ts;
176 mutable Vector m_A_times_q;
177 Eigen::DenseIndex m_rank;
Main pinocchio namespace.
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.