_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon =
std::numeric_limits<double>::epsilon())
{
Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU |
Eigen::ComputeThinV);
double tolerance = epsilon * std::max(a.cols(), a.rows())
*svd.singularValues().array().abs()(0);
return svd.matrixV() * (svd.singularValues().array().abs() >
tolerance).select(svd.singularValues().array().inverse(),
0).matrix().asDiagonal() * svd.matrixU().adjoint();
}
bool pseudoInverse(
const _Matrix_Type_ &a, _Matrix_Type_ &result,
double epsilon =
std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon()) {
if (a.rows() < a.cols())
return false;
Eigen::JacobiSVD<_Matrix_Type_> svd = a.jacobiSvd();
typename _Matrix_Type_::Scalar tolerance =
epsilon * std::max(a.cols(), a.rows()) *
svd.singularValues().array().abs().maxCoeff();
result = svd.matrixV() *
_Matrix_Type_(
_Matrix_Type_((svd.singularValues().array().abs() > tolerance)
.select(svd.singularValues().array().inverse(),
0)).diagonal()) *
svd.matrixU().adjoint();
}
