Commit 0f53ce3b authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Move hpp/model/eigen.hh to hpp/constraints/svd.hh

parent 879c7880
......@@ -61,7 +61,6 @@ 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>
static Eigen::Ref<const typename SVD::MatrixUType>
getU1 (const SVD& svd)
{
return svd.matrixU().leftCols (svd.rank());
}
template <typename SVD>
static Eigen::Ref<const typename SVD::MatrixUType>
getU2 (const SVD& svd)
{
return svd.matrixU().rightCols (svd.matrixU().cols() - svd.rank ());
}
template <typename SVD>
static Eigen::Ref<const typename SVD::MatrixUType>
getV1 (const SVD& svd)
{
return svd.matrixV().leftCols (svd.rank());
}
template <typename SVD>
static Eigen::Ref<const typename SVD::MatrixUType>
getV2 (const SVD& svd)
{
return svd.matrixV().rightCols (svd.matrixV().cols() - svd.rank ());
}
template < typename SVD>
static void pseudoInverse(const SVD& svd,
Eigen::Ref <typename SVD::MatrixType> pinvmat)
{
eigen_assert(svd.computeU() && svd.computeV() && "Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU | ComputeThinV");
typename SVD::SingularValuesType singularValues_inv =
svd.singularValues().segment (0,svd.rank()).cwiseInverse ();
pinvmat.noalias() =
getV1<SVD> (svd) * singularValues_inv.asDiagonal() *
getU1<SVD> (svd).adjoint();
}
template < typename SVD >
void projectorOnSpan (const SVD svd,
Eigen::Ref <typename SVD::MatrixType> projector)
{
eigen_assert(svd.computeU() && svd.computeV() && "Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU | ComputeThinV");
projector.noalias() = getV1<SVD> (svd) * getV1<SVD>(svd).adjoint();
}
template < typename SVD >
void projectorOnSpanOfInv (const SVD svd,
Eigen::Ref <typename SVD::MatrixType> projector)
{
eigen_assert(svd.computeU() && svd.computeV() && "Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU | ComputeThinV");
projector.noalias() = getU1<SVD>(svd) * getU1<SVD>(svd).adjoint();
}
template < typename SVD >
void projectorOnKernel (const SVD svd,
Eigen::Ref <typename SVD::MatrixType> projector,
const bool& computeFullV = false)
{
eigen_assert(svd.computeV() && "Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinV");
if (computeFullV)
projector.noalias() = getV2<SVD> (svd) * getV2<SVD>(svd).adjoint();
else {
projector.noalias() = - getV1<SVD> (svd) * getV1<SVD>(svd).adjoint();
projector.diagonal().noalias () += vector_t::Ones(svd.matrixV().rows());
}
}
template < typename SVD >
void projectorOnKernelOfInv (const SVD svd,
Eigen::Ref <typename SVD::MatrixType> projector,
const bool& computeFullU = false)
{
eigen_assert(svd.computeU() && "Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU");
if (computeFullU) {
// U2 * U2*
projector.noalias() = getU2<SVD>(svd) * getU2<SVD>(svd).adjoint();
} else {
// I - U1 * U1*
projector.noalias() = - getU1<SVD>(svd) * getU1<SVD>(svd).adjoint();
projector.diagonal().noalias () += vector_t::Ones(svd.matrixU().rows());
}
}
} // namespace model
} // namespace hpp
#endif // HPP_MODEL_EIGEN_HH
......@@ -44,4 +44,3 @@ 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 <Eigen/Dense>
#include <hpp/model/eigen.hh>
using hpp::model::pseudoInverse;
using hpp::model::projectorOnKernel;
using hpp::model::projectorOnKernelOfInv;
using hpp::model::projectorOnSpan;
using hpp::model::projectorOnSpanOfInv;
using hpp::model::matrix_t;
using hpp::model::value_type;
BOOST_AUTO_TEST_CASE(eigen_pseudoInverse_features)
{
const std::size_t rows = 4, cols = 6;
const value_type tol = 1e-6;
typedef Eigen::JacobiSVD <matrix_t> SVD;
const bool computeFullU = false;
const bool computeFullV = true;
int computationFlags =
( computeFullU ? Eigen::ComputeFullU : Eigen::ComputeThinU )
| ( computeFullV ? Eigen::ComputeFullV : Eigen::ComputeThinV );
SVD svd (rows, cols, computationFlags);
svd.setThreshold (tol);
matrix_t Mpinv (cols, rows);
for (int i = 0; i < 1000; ++i) {
matrix_t M = matrix_t::Random (rows, cols);
matrix_t PK (cols, cols);
matrix_t PS (cols, cols);
matrix_t PKinv (rows, rows);
matrix_t PSinv (rows, rows);
svd.compute (M);
BOOST_CHECK_MESSAGE ((svd.matrixV().adjoint() * svd.matrixV() - matrix_t::Identity (svd.matrixV().cols(),svd.matrixV().cols())).isZero(),
svd.matrixV().adjoint() * svd.matrixV() - matrix_t::Identity (svd.matrixV().cols(),svd.matrixV().cols()));
pseudoInverse <SVD> (svd, Mpinv);
projectorOnKernel <SVD> (svd, PK, computeFullV);
projectorOnSpan <SVD> (svd, PS);
projectorOnKernelOfInv <SVD> (svd, PKinv, computeFullU);
projectorOnSpanOfInv <SVD> (svd, PSinv);
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");
BOOST_CHECK_MESSAGE (PS.isApprox (Ic), "PK = M+ * M failed");
BOOST_CHECK_MESSAGE (PSinv.isApprox (Ir), "PKinv = M * M+ failed");
BOOST_CHECK_MESSAGE ((PS + PK) .isApprox (matrix_t::Identity(cols, cols)), "PS + PK = I failed");
BOOST_CHECK_MESSAGE ((PSinv + PKinv).isApprox (matrix_t::Identity(rows, rows)), "PSinv + PKinv = I 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