// @from http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00173.html static bool pinv(const Eigen::Matrix2d& a, Eigen::Matrix2d& a_pinv) { // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method if (a.rows() < a.cols()) return false; // SVD Eigen::JacobiSVD<Eigen::Matrix2d> svdA(a, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Vector2d vSingular = svdA.singularValues(); // Build a diagonal matrix with the Inverted Singular values // The pseudo inverted singular matrix is easy to compute : // is formed by replacing every nonzero entry by its reciprocal (inversing). Eigen::Vector2d vPseudoInvertedSingular(svdA.matrixV().cols(),1); for (int iRow = 0; iRow < vSingular.rows(); iRow++) { if (fabs(vSingular(iRow)) <= 1e-10) // Todo : Put epsilon in parameter { vPseudoInvertedSingular(iRow, 0) = 0.; } else { vPseudoInvertedSingular(iRow, 0) = 1. / vSingular(iRow); } } // A little optimization here Eigen::Matrix2d mAdjointU = svdA.matrixU().adjoint().block(0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols()); // Pseudo-Inversion : V * S * U' a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU; return true; }