// @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; }
bool MatrixXr_pseudoInverse(const MatrixXr &a, MatrixXr &a_pinv, double epsilon) { // 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<MatrixXr> svdA; svdA.compute(a,Eigen::ComputeThinU|Eigen::ComputeThinV); MatrixXr 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). VectorXr vPseudoInvertedSingular(svdA.matrixV().cols(),1); for (int iRow =0; iRow<vSingular.rows(); iRow++) { if(fabs(vSingular(iRow))<=epsilon) vPseudoInvertedSingular(iRow,0)=0.; else vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow); } // A little optimization here MatrixXr 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; }
// Derived from code by Yohann Solaro ( http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00187.html ) // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method Eigen::MatrixXd pinv( const Eigen::MatrixXd &b, double rcond ) { // TODO: Figure out why it wants fewer rows than columns // if ( a.rows()<a.cols() ) // return false; bool flip = false; Eigen::MatrixXd a; if( a.rows() < a.cols() ) { a = b.transpose(); flip = true; } else a = b; // SVD Eigen::JacobiSVD<Eigen::MatrixXd> svdA; svdA.compute( a, Eigen::ComputeFullU | Eigen::ComputeThinV ); Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType 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::VectorXd vPseudoInvertedSingular( svdA.matrixV().cols() ); for (int iRow=0; iRow<vSingular.rows(); iRow++) { if ( fabs(vSingular(iRow)) <= rcond ) { vPseudoInvertedSingular(iRow)=0.; } else vPseudoInvertedSingular(iRow)=1./vSingular(iRow); } // A little optimization here Eigen::MatrixXd mAdjointU = svdA.matrixU().adjoint().block( 0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols() ); // Pseudo-Inversion : V * S * U' Eigen::MatrixXd a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU; if( flip ) { a = a.transpose(); a_pinv = a_pinv.transpose(); } return a_pinv; }
// Derived from code by Yohann Solaro ( http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00187.html ) void pinv(const MatrixXd &b, MatrixXd &a_pinv) { // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method // TODO: Figure out why it wants fewer rows than columns // if ( a.rows()<a.cols() ) // return false; bool flip = false; MatrixXd a; if( a.rows() < a.cols() ) { a = b.transpose(); flip = true; } else a = b; // SVD JacobiSVD< MatrixXd > svdA; svdA.compute(a, ComputeFullU | ComputeThinV); JacobiSVD<MatrixXd>::SingularValuesType 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). VectorXd vPseudoInvertedSingular(svdA.matrixV().cols()); for (int iRow =0; iRow<vSingular.rows(); iRow++) { if ( fabs(vSingular(iRow))<=1e-10 ) // Todo : Put epsilon in parameter { vPseudoInvertedSingular(iRow)=0.; } else { vPseudoInvertedSingular(iRow)=1./vSingular(iRow); } } // A little optimization here MatrixXd 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; if(flip) { a = a.transpose(); a_pinv = a_pinv.transpose(); } }