Commit 42c6ba68 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Enhance functions to compute SVD related matrices

parent 2a6444bc
......@@ -24,70 +24,101 @@
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())
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");
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 ();
svd.singularValues().segment (0,svd.rank()).cwiseInverse ();
pinvmat = svd.matrixV () * singularValues_inv.asDiagonal()
* svd.matrixU().adjoint();
pinvmat.noalias() =
getV1<SVD> (svd) * singularValues_inv.asDiagonal() *
getU1<SVD> (svd).adjoint();
}
template < typename SVD >
void projectorOnKernel (const SVD svd,
Eigen::Ref <typename SVD::MatrixType> projector,
const value_type tolerance =
Eigen::NumTraits<typename SVD::MatrixType::Scalar>::epsilon())
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");
const typename SVD::SingularValuesType& singularValues
= svd.singularValues ();
projector.noalias() = getV1<SVD> (svd) * getV1<SVD>(svd).adjoint();
}
typename SVD::SingularValuesType sv_invTimesSv =
(singularValues.array () >= tolerance).select (
SVD::SingularValuesType::Ones (singularValues.size()),
SVD::SingularValuesType::Zero (singularValues.size())
).matrix ();
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 = svd.matrixV () * sv_invTimesSv.asDiagonal()
* svd.matrixV().adjoint();
projector.noalias() = getU1<SVD>(svd) * getU1<SVD>(svd).adjoint();
}
template < typename SVD >
void projectorOnKernelOfInv (const SVD svd,
void projectorOnKernel (const SVD svd,
Eigen::Ref <typename SVD::MatrixType> projector,
const value_type tolerance =
Eigen::NumTraits<typename SVD::MatrixType::Scalar>::epsilon())
const bool& computeFullV = false)
{
eigen_assert(svd.computeU() && svd.computeV() && "Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinU | ComputeThinV");
eigen_assert(svd.computeV() && "Eigen::JacobiSVD "
"computation flags must be at least: ComputeThinV");
const typename SVD::SingularValuesType& singularValues
= svd.singularValues ();
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());
}
}
typename SVD::SingularValuesType sv_invTimesSv =
(singularValues.array () >= tolerance).select (
SVD::SingularValuesType::Ones (singularValues.size()),
SVD::SingularValuesType::Zero (singularValues.size())
).matrix ();
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");
projector = svd.matrixU () * sv_invTimesSv.asDiagonal()
* svd.matrixU().adjoint();
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
......
......@@ -22,25 +22,40 @@
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(testPseudoInverse)
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;
SVD svd (rows, cols, Eigen::ComputeThinU | Eigen::ComputeThinV);
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);
pseudoInverse <SVD> (svd, Mpinv, tol);
projectorOnKernel <SVD> (svd, PK, tol);
projectorOnKernelOfInv <SVD> (svd, PKinv, tol);
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;
......@@ -50,7 +65,10 @@ BOOST_AUTO_TEST_CASE(testPseudoInverse)
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 (PK.isApprox (Ic), "PK = M+ * M failed");
BOOST_CHECK_MESSAGE (PKinv.isApprox (Ir), "PKinv = 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