From 051450e904a7f5a0d9008ef000d5063084943ce2 Mon Sep 17 00:00:00 2001
From: asantamaria <somriu@gmail.com>
Date: Tue, 30 Aug 2016 12:47:36 +0200
Subject: [PATCH] removed cout and added new CalcPinv cleaned
---
src/common_fc.cpp | 49 ++++------------------------------------------
src/kinematics.cpp | 2 --
2 files changed, 4 insertions(+), 47 deletions(-)
diff --git a/src/common_fc.cpp b/src/common_fc.cpp
index 16b4ecd..470e8a4 100644
--- a/src/common_fc.cpp
+++ b/src/common_fc.cpp
@@ -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;
}
diff --git a/src/kinematics.cpp b/src/kinematics.cpp
index 2965b76..5e7cf9d 100644
--- a/src/kinematics.cpp
+++ b/src/kinematics.cpp
@@ -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);
--
GitLab