pinv.hh

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
#include <Eigen/SVD>
 
Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& a, double epsilon = std::numeric_limits<double>::epsilon())
{
    Eigen::JacobiSVD< Eigen::MatrixXd > 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();
}
 
Eigen::MatrixXd weightedPseudoInverse(const Eigen::MatrixXd& a, const Eigen::VectorXd w)
{
 
    int lenght = w.size();
 
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> Winv(lenght);
    Winv = w.asDiagonal().inverse();  // diag(1./w)
 
    Eigen::MatrixXd tmp(lenght, lenght);
 
    //(a*Winv*a.transpose())
    tmp = pseudoInverse(a * Winv * a.transpose(), 10E-10);
 
    return Winv * a.transpose() * tmp;
 
}