18 #include <Eigen/Dense> 27 template <
typename _Matrix_Type_>
29 Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
30 _Matrix_Type_ m_sigma = svd.singularValues();
32 double pinvtoler = 1.e-6;
34 _Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
35 for (
long i = 0; i < m_sigma.rows(); ++i) {
36 if (m_sigma(i) > pinvtoler) m_sigma_inv(i, i) = 1.0 / m_sigma(i);
38 pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
void PseudoInverse(_Matrix_Type_ &pinvmat)
Definition: MathDefs.h:28
Definition: MathDefs.h:24