Add pseudoInverse algorithm

CMakeLists.txt

@@ -61,6 +61,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/model/gripper.hh
   include/hpp/model/center-of-mass-computation.hh
   include/hpp/model/debug.hh
+  include/hpp/model/eigen.hh
   )
 
 # Declare dependencies

include/hpp/model/eigen.hh

+// Copyright (c) 2015 CNRS
+// Author: Joseph Mirabel
+//
+//
+// This file is part of hpp-model
+// hpp-model is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-model is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-model  If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MODEL_EIGEN_HH
+# define HPP_MODEL_EIGEN_HH
+
+# include <hpp/model/fwd.hh>
+# include <Eigen/SVD>
+
+namespace hpp {
+  namespace model {
+    template < typename SVD >
+    void pseudoInverse(const SVD& svd,
+        Eigen::Ref <typename SVD::MatrixType> pinvmat,
+        const value_type tolerance =
+        Eigen::NumTraits<typename SVD::MatrixType::Scalar>::epsilon())
+    {
+      eigen_assert(svd.computeU() && svd.computeV() && "Eigen::JacobiSVD "
+          "computation flags must be at least: ComputeThinU | ComputeThinV");
+
+      const typename SVD::SingularValuesType& singularValues
+        = svd.singularValues ();
+
+      typename SVD::SingularValuesType singularValues_inv =
+        (singularValues.array () >= tolerance).select (
+            singularValues.array ().cwiseInverse (),
+            SVD::SingularValuesType::Zero (singularValues.size())
+            ).matrix ();
+
+      pinvmat = svd.matrixV () * singularValues_inv.asDiagonal()
+        * svd.matrixU().adjoint();
+    }
+    }
+  } // namespace model
+} // namespace hpp
+
+#endif // HPP_MODEL_EIGEN_HH

tests/CMakeLists.txt

@@ -44,3 +44,4 @@ MACRO(HPP_MODEL_TEST NAME)
 ENDMACRO(HPP_MODEL_TEST)
 
 HPP_MODEL_TEST (test-configuration)
+HPP_MODEL_TEST (eigen)

tests/eigen.cc

+// Copyright (c) 2015, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-model.
+// hpp-model is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-model is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-model. If not, see <http://www.gnu.org/licenses/>.
+
+#define BOOST_TEST_MODULE EIGEN_FEATURE
+#include <boost/test/unit_test.hpp>
+
+#include <hpp/model/eigen.hh>
+
+using hpp::model::pseudoInverse;
+using hpp::model::matrix_t;
+using hpp::model::value_type;
+
+BOOST_AUTO_TEST_CASE(testPseudoInverse)
+{
+  const std::size_t rows = 4, cols = 6;
+  const value_type tol = 1e-6;
+  typedef Eigen::JacobiSVD <matrix_t> SVD;
+  SVD svd (rows, cols, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  matrix_t Mpinv (cols, rows);
+  for (int i = 0; i < 1000; ++i) {
+    matrix_t M = matrix_t::Random (rows, cols);
+    svd.compute (M);
+    pseudoInverse <SVD> (svd, Mpinv, tol); 
+    matrix_t Ir = M * Mpinv;
+    matrix_t Ic = Mpinv * M;
+    matrix_t _M = M * Ic;
+    matrix_t _Mpinv = Mpinv * Ir;
+    BOOST_CHECK_MESSAGE (_M.isApprox (M), "M = M * M+ * M failed");
+    BOOST_CHECK_MESSAGE (_Mpinv.isApprox (Mpinv), "M+ = M+ * M * M+ failed");
+    BOOST_CHECK_MESSAGE (Ir.adjoint ().isApprox (Ir), "(M * M+)* = M * M+ failed");
+    BOOST_CHECK_MESSAGE (Ic.adjoint ().isApprox (Ic), "(M+ * M)* = M+ * M failed");
+  }
+}