Skip to content
Snippets Groups Projects
Commit 051450e9 authored by asantamaria's avatar asantamaria
Browse files

removed cout and added new CalcPinv cleaned

parent b99d90fd
No related branches found
No related tags found
No related merge requests found
......@@ -8,51 +8,10 @@ CCommon_Fc::~CCommon_Fc(){}
Eigen::MatrixXd CCommon_Fc::CalcPinv(const Eigen::MatrixXd& a, double epsilon)
{
// Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
// 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();
// 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;
Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
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();
return mpinv;
}
......
......@@ -24,8 +24,6 @@ void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT,
// Quadrotor Jacobian body to inertial frames
Eigen::MatrixXd Jb2i = QuadJac(quad.pos);
std::cout << __LINE__ << " [" << quad.pos.transpose() << "]" << std::endl;
// Inertial to body frames
Eigen::MatrixXd Ji2b = UAM::CCommon_Fc::CalcPinv(Jb2i);
......
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