Commit f23b2e8c authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add pseudoInverse algorithm

parent 3a6dc6ab
......@@ -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
......
// 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
......@@ -44,3 +44,4 @@ MACRO(HPP_MODEL_TEST NAME)
ENDMACRO(HPP_MODEL_TEST)
HPP_MODEL_TEST (test-configuration)
HPP_MODEL_TEST (eigen)
// 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");
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment