Program Listing for File eigenvalues.hpp
↰ Return to documentation for file (include/pinocchio/math/eigenvalues.hpp
)
//
// Copyright (c) 2022-2024 INRIA
//
#ifndef __pinocchio_math_eigenvalues_hpp__
#define __pinocchio_math_eigenvalues_hpp__
#include "pinocchio/math/fwd.hpp"
#include <Eigen/Core>
namespace pinocchio
{
template<typename _Vector>
struct PowerIterationAlgoTpl
{
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(_Vector) Vector;
typedef typename Vector::Scalar Scalar;
explicit PowerIterationAlgoTpl(
const Eigen::DenseIndex size, const int max_it = 10, const Scalar rel_tol = Scalar(1e-8))
: principal_eigen_vector(size)
, lowest_eigen_vector(size)
, max_it(max_it)
, it(0)
, rel_tol(rel_tol)
, eigen_vector_prev(size)
{
reset();
}
template<typename MatrixLike>
void run(const MatrixLike & mat)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(max_it >= 1);
PINOCCHIO_CHECK_INPUT_ARGUMENT(rel_tol > Scalar(0));
Scalar eigenvalue_est = principal_eigen_vector.norm();
for (it = 0; it < max_it; ++it)
{
const Scalar eigenvalue_est_prev = eigenvalue_est;
principal_eigen_vector /= eigenvalue_est;
eigen_vector_prev = principal_eigen_vector;
principal_eigen_vector.noalias() = mat * eigen_vector_prev;
eigenvalue_est = principal_eigen_vector.norm();
convergence_criteria = math::fabs(eigenvalue_est_prev - eigenvalue_est);
if (check_expression_if_real<Scalar, false>(
convergence_criteria
<= rel_tol * math::max(math::fabs(eigenvalue_est_prev), math::fabs(eigenvalue_est))))
break;
}
largest_eigen_value = eigenvalue_est;
}
template<typename MatrixLike, typename VectorLike>
void run(const MatrixLike & mat, const Eigen::PlainObjectBase<VectorLike> & eigenvector_est)
{
principal_eigen_vector = eigenvector_est;
run(mat);
}
template<typename MatrixLike>
void lowest(const MatrixLike & mat, const bool compute_largest = true)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(max_it >= 1);
PINOCCHIO_CHECK_INPUT_ARGUMENT(rel_tol > Scalar(0));
if (compute_largest)
run(mat);
Scalar eigenvalue_est = lowest_eigen_vector.norm();
for (it = 0; it < max_it; ++it)
{
const Scalar eigenvalue_est_prev = eigenvalue_est;
lowest_eigen_vector /= eigenvalue_est;
eigen_vector_prev = lowest_eigen_vector;
lowest_eigen_vector.noalias() = mat * eigen_vector_prev;
lowest_eigen_vector -= largest_eigen_value * eigen_vector_prev;
eigenvalue_est = lowest_eigen_vector.norm();
convergence_criteria = math::fabs(eigenvalue_est_prev - eigenvalue_est);
if (check_expression_if_real<Scalar, false>(
convergence_criteria
<= rel_tol * math::max(math::fabs(eigenvalue_est_prev), math::fabs(eigenvalue_est))))
break;
}
lowest_eigen_value = largest_eigen_value - eigenvalue_est;
}
template<typename MatrixLike, typename VectorLike>
void lowest(
const MatrixLike & mat,
const Eigen::PlainObjectBase<VectorLike> & largest_eigenvector_est,
const Eigen::PlainObjectBase<VectorLike> & lowest_eigenvector_est,
const bool compute_largest = true)
{
principal_eigen_vector = largest_eigenvector_est;
lowest_eigen_vector = lowest_eigenvector_est;
lowest(mat, compute_largest);
}
void reset()
{
const Scalar normalized_value = Scalar(1) / math::sqrt(Scalar(principal_eigen_vector.size()));
principal_eigen_vector.fill(normalized_value);
lowest_eigen_vector.fill(normalized_value);
largest_eigen_value = std::numeric_limits<Scalar>::min();
lowest_eigen_value = std::numeric_limits<Scalar>::max();
}
Vector principal_eigen_vector;
Vector lowest_eigen_vector;
Scalar largest_eigen_value;
Scalar lowest_eigen_value;
int max_it;
int it;
Scalar rel_tol;
Scalar convergence_criteria;
protected:
Vector eigen_vector_prev;
}; // struct PowerIterationAlgoTpl
template<typename MatrixLike, typename VectorLike>
void computeLargestEigenvector(
const MatrixLike & mat,
const Eigen::PlainObjectBase<VectorLike> & _eigenvector_est,
const int max_it = 10,
const typename MatrixLike::Scalar rel_tol = 1e-8)
{
PowerIterationAlgoTpl<VectorLike> algo(mat.rows(), max_it, rel_tol);
algo.run(mat, _eigenvector_est.derived());
_eigenvector_est.const_cast_derived() = algo.principal_eigen_vector;
}
template<typename MatrixLike>
Eigen::Matrix<typename MatrixLike::Scalar, MatrixLike::RowsAtCompileTime, 1>
computeLargestEigenvector(
const MatrixLike & mat, const int max_it = 10, const typename MatrixLike::Scalar rel_tol = 1e-8)
{
typedef Eigen::Matrix<typename MatrixLike::Scalar, MatrixLike::RowsAtCompileTime, 1> Vector;
PowerIterationAlgoTpl<Vector> algo(mat.rows(), max_it, rel_tol);
algo.run(mat);
return algo.principal_eigen_vector;
}
template<typename MatrixLike, typename VectorLike1, typename VectorLike2>
void computeLowestEigenvector(
const MatrixLike & mat,
const Eigen::PlainObjectBase<VectorLike1> & largest_eigenvector_est,
const Eigen::PlainObjectBase<VectorLike2> & lowest_eigenvector_est,
const bool compute_largest = true,
const int max_it = 10,
const typename MatrixLike::Scalar rel_tol = 1e-8)
{
PowerIterationAlgoTpl<VectorLike1> algo(mat.rows(), max_it, rel_tol);
algo.lowest(
mat, largest_eigenvector_est.derived(), lowest_eigenvector_est.derived(), compute_largest);
largest_eigenvector_est.const_cast_derived() = algo.principal_eigen_vector;
lowest_eigenvector_est.const_cast_derived() = algo.lowest_eigen_vector;
}
template<typename MatrixLike>
Eigen::Matrix<typename MatrixLike::Scalar, MatrixLike::RowsAtCompileTime, 1>
computeLowestEigenvector(
const MatrixLike & mat,
const bool compute_largest = true,
const int max_it = 10,
const typename MatrixLike::Scalar rel_tol = 1e-8)
{
typedef Eigen::Matrix<typename MatrixLike::Scalar, MatrixLike::RowsAtCompileTime, 1> Vector;
PowerIterationAlgoTpl<Vector> algo(mat.rows(), max_it, rel_tol);
algo.lowest(mat, compute_largest);
return algo.lowest_eigen_vector;
}
template<typename VectorLike>
typename VectorLike::Scalar
retrieveLargestEigenvalue(const Eigen::MatrixBase<VectorLike> & eigenvector)
{
return eigenvector.norm();
}
} // namespace pinocchio
#endif // #ifndef __pinocchio_math_eigenvalues_hpp__