Program Listing for File lanczos-decomposition.hpp
↰ Return to documentation for file (include/pinocchio/math/lanczos-decomposition.hpp
)
//
// Copyright (c) 2024 INRIA
//
#ifndef __pinocchio_math_lanczos_decomposition_hpp__
#define __pinocchio_math_lanczos_decomposition_hpp__
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/tridiagonal-matrix.hpp"
#include "pinocchio/math/gram-schmidt-orthonormalisation.hpp"
namespace pinocchio
{
template<typename _Matrix>
struct LanczosDecompositionTpl
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(_Matrix) PlainMatrix;
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(typename PlainMatrix::ColXpr) Vector;
typedef typename _Matrix::Scalar Scalar;
enum
{
Options = _Matrix::Options
};
typedef TridiagonalSymmetricMatrixTpl<Scalar, Options> TridiagonalMatrix;
template<typename MatrixLikeType>
LanczosDecompositionTpl(const MatrixLikeType & mat, const Eigen::DenseIndex decomposition_size)
: m_Qs(mat.rows(), decomposition_size)
, m_Ts(decomposition_size)
, m_A_times_q(mat.rows())
, m_rank(-1)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(mat.rows() == mat.cols(), "The input matrix is not square.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(
decomposition_size >= 1, "The size of the decomposition should be greater than one.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(
decomposition_size <= mat.rows(),
"The size of the decomposition should not be larger than the number of rows.");
compute(mat);
}
bool operator==(const LanczosDecompositionTpl & other) const
{
if (this == &other)
return true;
return m_Qs == other.m_Qs && m_Ts == other.m_Ts && m_rank == other.m_rank;
}
bool operator!=(const LanczosDecompositionTpl & other) const
{
return !(*this == other);
}
template<typename MatrixLikeType>
void compute(const MatrixLikeType & A)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(A.rows() == A.cols(), "The input matrix is not square.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(
A.rows() == m_Qs.rows(), "The input matrix is of correct size.");
const Eigen::DenseIndex decomposition_size = m_Ts.cols();
auto & alphas = m_Ts.diagonal();
auto & betas = m_Ts.subDiagonal();
m_Qs.col(0).fill(Scalar(1) / math::sqrt(Scalar(m_Qs.rows())));
m_Ts.setZero();
Eigen::DenseIndex k;
for (k = 0; k < decomposition_size; ++k)
{
const auto q = m_Qs.col(k);
m_A_times_q.noalias() = A * q;
alphas[k] = q.dot(m_A_times_q);
if (k < decomposition_size - 1)
{
auto q_next = m_Qs.col(k + 1);
m_A_times_q -= alphas[k] * q;
if (k > 0)
{
m_A_times_q -= betas[k - 1] * m_Qs.col(k - 1);
}
// Perform Gram-Schmidt orthogonalization procedure.
if (k > 0)
orthonormalisation(m_Qs.leftCols(k), m_A_times_q);
// Compute beta
betas[k] = m_A_times_q.norm();
if (betas[k] <= 1e2 * Eigen::NumTraits<Scalar>::epsilon())
{
break;
}
else
{
q_next.noalias() = m_A_times_q / betas[k];
}
}
}
m_rank = math::max(Eigen::DenseIndex(1), k);
}
template<typename MatrixLikeType>
PlainMatrix computeDecompositionResidual(const MatrixLikeType & A) const
{
const Eigen::DenseIndex last_col_id = m_Ts.cols() - 1;
const auto & alphas = m_Ts.diagonal();
const auto & betas = m_Ts.subDiagonal();
PlainMatrix residual = A * m_Qs;
residual -= (m_Qs * m_Ts).eval();
const auto & q = m_Qs.col(last_col_id);
auto & tmp_vec = m_A_times_q; // use m_A_times_q as a temporary vector
tmp_vec.noalias() = A * q;
tmp_vec -= alphas[last_col_id] * q;
if (last_col_id > 0)
tmp_vec -= betas[last_col_id - 1] * m_Qs.col(last_col_id - 1);
residual.col(last_col_id) -= tmp_vec;
return residual;
}
const TridiagonalMatrix & Ts() const
{
return m_Ts;
}
TridiagonalMatrix & Ts()
{
return m_Ts;
}
const PlainMatrix & Qs() const
{
return m_Qs;
}
PlainMatrix & Qs()
{
return m_Qs;
}
Eigen::DenseIndex rank() const
{
return m_rank;
}
protected:
PlainMatrix m_Qs;
TridiagonalMatrix m_Ts;
mutable Vector m_A_times_q;
Eigen::DenseIndex m_rank;
};
} // namespace pinocchio
#endif // ifndef __pinocchio_math_lanczos_decomposition_hpp__