コード例 #1
4
ファイル: unicycle_motions.cpp プロジェクト: aurone/mprims
// @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;
}