testing old calcpinv

src/common_fc.cpp

@@ -8,10 +8,51 @@ 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;
+  // 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;
 }