util.h

Go to the documentation of this file.

#ifndef UTIL_H
#define UTIL_H
#include <Eigen/Core>
#include <Eigen/SVD>


template<typename _Matrix_Type_>
bool pseudoInverse(const _Matrix_Type_ &a, _Matrix_Type_ &result, double epsilon = std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon())
{
    Eigen::JacobiSVD< _Matrix_Type_ > svd = a.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);

    typename _Matrix_Type_::Scalar tolerance = epsilon * std::max(a.cols(), a.rows()) *
                                               svd.singularValues().array().abs().maxCoeff();

    result = svd.matrixV() * _Matrix_Type_( (svd.singularValues().array().abs() >
             tolerance).select(svd.singularValues().array().inverse(), 0) ).asDiagonal() *
             svd.matrixU().adjoint();
}

#endif // UTIL_H