6 #ifndef CNOID_UTIL_TRUNCATED_SVD_SOLVER_H_INCLUDED 7 #define CNOID_UTIL_TRUNCATED_SVD_SOLVER_H_INCLUDED 13 template <
class MatrixType>
16 typedef typename MatrixType::Scalar Scalar;
17 Eigen::JacobiSVD<MatrixType> svd;
18 Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic> sinv;
33 truncateRatio = std::numeric_limits<double>::infinity();
43 svd.
compute(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
46 int lastNonZeroSingularValues = svd.nonzeroSingularValues() - 1;
47 if(lastNonZeroSingularValues > 0){
48 Scalar& s0 = sinv.diagonal()(0);
49 Scalar& s_last = sinv.diagonal()(lastNonZeroSingularValues);
50 if((s0 / s_last) > truncateRatio){
54 for(j = lastNonZeroSingularValues - 1; j > 0; --j){
55 Scalar& s = sinv.diagonal()(j);
56 if((s0 / s) > truncateRatio){
64 Scalar& s = sinv.diagonal()(j);
77 const typename Eigen::JacobiSVD<MatrixType>::SingularValuesType&
singularValues()
const {
78 return svd.singularValues();
81 template <
class VectorType1,
class VectorType2>
82 void solve(
const Eigen::MatrixBase<VectorType1>& b, Eigen::MatrixBase<VectorType2>& out_x)
const {
84 out_x = svd.matrixV() * sinv * svd.matrixU().transpose() * b;
Definition: TruncatedSVD.h:14
int numTruncatedSingularValues() const
Definition: TruncatedSVD.h:73
void solve(const Eigen::MatrixBase< VectorType1 > &b, Eigen::MatrixBase< VectorType2 > &out_x) const
Definition: TruncatedSVD.h:82
void setTruncateRatio(Scalar r)
Definition: TruncatedSVD.h:31
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
TruncatedSVD & compute(const MatrixType &matrix)
Definition: TruncatedSVD.h:39
static double defaultTruncateRatio()
Definition: TruncatedSVD.h:24
const Eigen::JacobiSVD< MatrixType >::SingularValuesType & singularValues() const
Definition: TruncatedSVD.h:77
TruncatedSVD()
Definition: TruncatedSVD.h:26