Skip to content
Snippets Groups Projects
Commit 3eda6fed authored by asantamaria's avatar asantamaria
Browse files

test

parent 051450e9
No related branches found
No related tags found
No related merge requests found
...@@ -8,10 +8,51 @@ CCommon_Fc::~CCommon_Fc(){} ...@@ -8,10 +8,51 @@ CCommon_Fc::~CCommon_Fc(){}
Eigen::MatrixXd CCommon_Fc::CalcPinv(const Eigen::MatrixXd& a, double epsilon) Eigen::MatrixXd CCommon_Fc::CalcPinv(const Eigen::MatrixXd& a, double epsilon)
{ {
Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV); // Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0); // double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0);
Eigen::MatrixXd mpinv = svdd.matrixV() * (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint(); // Eigen::MatrixXd mpinv = svdd.matrixV() * (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint();
return mpinv; // return mpinv;
// see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
const Eigen::MatrixXd* m;
Eigen::MatrixXd t;
Eigen::MatrixXd m_pinv;
// transpose so SVD decomp can work...
if ( a.rows()<a.cols() )
{
t = a.transpose();
m = &t;
}
else
m = &a;
// SVD
//JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::MatrixXd vSingular = svd.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::MatrixXd vPseudoInvertedSingular(svd.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
Eigen::MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
// Pseudo-Inversion : V * S * U'
m_pinv = (svd.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU ;
// transpose back if necessary
if ( a.rows()<a.cols() )
return m_pinv.transpose();
return m_pinv;
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment