symmetric.cpp

Go to the documentation of this file.

//
// Copyright (c) 2015-2020 CNRS INRIA
//
 
/* --- Unitary test symmetric.cpp This code tests and compares two ways of
 * expressing symmetric matrices. In addition to the unitary validation (test
 * of the basic operations), the code is validating the computation
 * performances of each methods.
 *
 * The three methods are:
 * - Eigen SelfAdjoint (a mask atop of a classical dense matrix) ==> the least efficient.
 * - Pinocchio rewritting of Metapod code with LTI factor as well and minor improvement.
 *
 * IMPORTANT: the following timings seems outdated.
 * Expected time scores on a I7 2.1GHz:
 * - Eigen: 2.5us
 * - Pinocchio: 6us
 */
 
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/skew.hpp"
#include "pinocchio/utils/timer.hpp"
 
#include <boost/random.hpp>
#include <Eigen/Geometry>
 
#include "pinocchio/spatial/symmetric3.hpp"
 
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
 
void timeSym3(
  const pinocchio::Symmetric3 & S,
  const pinocchio::Symmetric3::Matrix3 & R,
  pinocchio::Symmetric3 & res)
{
  res = S.rotate(R);
}
 
#ifdef WITH_METAPOD
 
  #include <metapod/tools/spatial/lti.hh>
  #include <metapod/tools/spatial/rm-general.hh>
 
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::ltI<double>)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::RotationMatrixTpl<double>)
 
void timeLTI(
  const metapod::Spatial::ltI<double> & S,
  const metapod::Spatial::RotationMatrixTpl<double> & R,
  metapod::Spatial::ltI<double> & res)
{
  res = R.rotTSymmetricMatrix(S);
}
 
#endif
 
void timeSelfAdj(const Eigen::Matrix3d & A, const Eigen::Matrix3d & Sdense, Eigen::Matrix3d & ASA)
{
  typedef Eigen::SelfAdjointView<const Eigen::Matrix3d, Eigen::Upper> Sym3;
  Sym3 S(Sdense);
  ASA.triangularView<Eigen::Upper>() = A * S * A.transpose();
}
 
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
 
/* --- PINOCCHIO ------------------------------------------------------------ */
/* --- PINOCCHIO ------------------------------------------------------------ */
/* --- PINOCCHIO ------------------------------------------------------------ */
BOOST_AUTO_TEST_CASE(test_pinocchio_Sym3)
{
  using namespace pinocchio;
  typedef Symmetric3::Matrix3 Matrix3;
  typedef Symmetric3::Vector3 Vector3;
 
  {
    // op(Matrix3)
    {
      Matrix3 M = Matrix3::Random();
      M = M * M.transpose();
      Symmetric3 S(M);
      BOOST_CHECK(S.matrix().isApprox(M, 1e-12));
    }
 
    // S += S
    {
      Symmetric3 S = Symmetric3::Random(), S2 = Symmetric3::Random();
      Symmetric3 Scopy = S;
      S += S2;
      BOOST_CHECK(S.matrix().isApprox(S2.matrix() + Scopy.matrix(), 1e-12));
    }
 
    // S + M
    {
      Symmetric3 S = Symmetric3::Random();
      Matrix3 M = Matrix3::Random();
      M = M * M.transpose();
 
      Symmetric3 S2 = S + M;
      BOOST_CHECK(S2.matrix().isApprox(S.matrix() + M, 1e-12));
 
      S2 = S - M;
      BOOST_CHECK(S2.matrix().isApprox(S.matrix() - M, 1e-12));
    }
 
    // S*v
    {
      Symmetric3 S = Symmetric3::Random();
      Vector3 v = Vector3::Random();
      Vector3 Sv = S * v;
      BOOST_CHECK(Sv.isApprox(S.matrix() * v, 1e-12));
    }
 
    // Random
    for (int i = 0; i < 100; ++i)
    {
      Matrix3 M = Matrix3::Random();
      M = M * M.transpose();
      Symmetric3 S = Symmetric3::RandomPositive();
      Vector3 v = Vector3::Random();
      BOOST_CHECK_GT((v.transpose() * (S * v))[0], 0);
    }
 
    // Identity
    {
      BOOST_CHECK(Symmetric3::Identity().matrix().isApprox(Matrix3::Identity(), 1e-12));
    }
 
    // Set diagonal
    {
      Symmetric3 S0 = Symmetric3::Zero();
      const Symmetric3::Vector3 diag_elt =
        (Symmetric3::Vector3::Constant(1.) + Symmetric3::Vector3::Random());
      S0.setDiagonal(diag_elt);
 
      BOOST_CHECK(S0.matrix().diagonal().isApprox(diag_elt));
    }
 
    // Skew2
    {
      Vector3 v = Vector3::Random();
      Symmetric3 vxvx = Symmetric3::SkewSquare(v);
 
      Vector3 p = Vector3::UnitX();
      BOOST_CHECK((vxvx * p).isApprox(v.cross(v.cross(p)), 1e-12));
 
      p = Vector3::UnitY();
      BOOST_CHECK((vxvx * p).isApprox(v.cross(v.cross(p)), 1e-12));
 
      p = Vector3::UnitZ();
      BOOST_CHECK((vxvx * p).isApprox(v.cross(v.cross(p)), 1e-12));
 
      Matrix3 vx = skew(v);
      Matrix3 vxvx2 = (vx * vx).eval();
      BOOST_CHECK(vxvx.matrix().isApprox(vxvx2, 1e-12));
 
      Symmetric3 S = Symmetric3::RandomPositive();
      BOOST_CHECK((S - Symmetric3::SkewSquare(v)).matrix().isApprox(S.matrix() - vxvx2, 1e-12));
 
      double m = Eigen::internal::random<double>() + 1;
      BOOST_CHECK(
        (S - m * Symmetric3::SkewSquare(v)).matrix().isApprox(S.matrix() - m * vxvx2, 1e-12));
 
      Symmetric3 S2 = S;
      S -= Symmetric3::SkewSquare(v);
      BOOST_CHECK(S.matrix().isApprox(S2.matrix() - vxvx2, 1e-12));
 
      S = S2;
      S -= m * Symmetric3::SkewSquare(v);
      BOOST_CHECK(S.matrix().isApprox(S2.matrix() - m * vxvx2, 1e-12));
    }
 
    // (i,j)
    {
      Matrix3 M = Matrix3::Random();
      M = M * M.transpose();
      Symmetric3 S(M);
      for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
          BOOST_CHECK_SMALL(S(i, j) - M(i, j), Eigen::NumTraits<double>::dummy_precision());
    }
  }
 
  // SRS
  {
    Symmetric3 S = Symmetric3::RandomPositive();
    Matrix3 R = (Eigen::Quaterniond(Eigen::Matrix<double, 4, 1>::Random())).normalized().matrix();
 
    Symmetric3 RSRt = S.rotate(R);
    BOOST_CHECK(RSRt.matrix().isApprox(R * S.matrix() * R.transpose(), 1e-12));
 
    Symmetric3 RtSR = S.rotate(R.transpose());
    BOOST_CHECK(RtSR.matrix().isApprox(R.transpose() * S.matrix() * R, 1e-12));
  }
 
  // Test operator vtiv
  {
    Symmetric3 S = Symmetric3::RandomPositive();
    Vector3 v = Vector3::Random();
    double kinetic_ref = v.transpose() * S.matrix() * v;
    double kinetic = S.vtiv(v);
    BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
  }
 
  // Test v x S3
  {
    Symmetric3 S = Symmetric3::RandomPositive();
    Vector3 v = Vector3::Random();
    Matrix3 Vcross = skew(v);
    Matrix3 M_ref(Vcross * S.matrix());
 
    Matrix3 M_res;
    Symmetric3::vxs(v, S, M_res);
    BOOST_CHECK(M_res.isApprox(M_ref));
 
    BOOST_CHECK(S.vxs(v).isApprox(M_ref));
  }
 
  // Test S3 vx
  {
    Symmetric3 S = Symmetric3::RandomPositive();
    Vector3 v = Vector3::Random();
    Matrix3 Vcross = skew(v);
    Matrix3 M_ref(S.matrix() * Vcross);
 
    Matrix3 M_res;
    Symmetric3::svx(v, S, M_res);
    BOOST_CHECK(M_res.isApprox(M_ref));
 
    BOOST_CHECK(S.svx(v).isApprox(M_ref));
  }
 
  // Test isZero
  {
    Symmetric3 S_not_zero = Symmetric3::Identity();
    BOOST_CHECK(!S_not_zero.isZero());
 
    Symmetric3 S_zero = Symmetric3::Zero();
    BOOST_CHECK(S_zero.isZero());
  }
 
  // Test isApprox
  {
    Symmetric3 S1 = Symmetric3::RandomPositive();
    Symmetric3 S2 = S1;
 
    BOOST_CHECK(S1.isApprox(S2));
 
    Symmetric3 S3 = S1;
    S3 += S3;
    BOOST_CHECK(!S1.isApprox(S3));
  }
 
  // Test inverse
  {
    Symmetric3 S1 = Symmetric3::RandomPositive();
    Symmetric3::Matrix3 inv = S1.inverse();
 
    BOOST_CHECK(inv.isApprox(S1.matrix().inverse()));
  }
 
  // Time test
  {
    const size_t NBT = 100000;
    Symmetric3 S = Symmetric3::RandomPositive();
 
    std::vector<Symmetric3> Sres(NBT);
    std::vector<Matrix3> Rs(NBT);
    for (size_t i = 0; i < NBT; ++i)
      Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double, 4, 1>::Random())).normalized().matrix();
 
    std::cout << "Pinocchio: ";
    PinocchioTicToc timer(PinocchioTicToc::US);
    timer.tic();
    SMOOTH(NBT)
    {
      timeSym3(S, Rs[_smooth], Sres[_smooth]);
    }
    timer.toc(std::cout, NBT);
  }
}
 
/* --- EIGEN SYMMETRIC ------------------------------------------------------ */
/* --- EIGEN SYMMETRIC ------------------------------------------------------ */
/* --- EIGEN SYMMETRIC ------------------------------------------------------ */
 
BOOST_AUTO_TEST_CASE(test_eigen_SelfAdj)
{
  using namespace pinocchio;
  typedef Eigen::Matrix3d Matrix3;
  typedef Eigen::SelfAdjointView<Matrix3, Eigen::Upper> Sym3;
 
  Matrix3 M = Matrix3::Random();
  Sym3 S(M);
  {
    Matrix3 Scp = S;
    BOOST_CHECK((Scp - Scp.transpose()).isApprox(Matrix3::Zero(), 1e-16));
  }
 
  Matrix3 M2 = Matrix3::Random();
  M.triangularView<Eigen::Upper>() = M2;
 
  Matrix3 A = Matrix3::Random(), ASA1, ASA2;
  ASA1.triangularView<Eigen::Upper>() = A * S * A.transpose();
  timeSelfAdj(A, M, ASA2);
 
  {
    Matrix3 Masa1 = ASA1.selfadjointView<Eigen::Upper>();
    Matrix3 Masa2 = ASA2.selfadjointView<Eigen::Upper>();
    BOOST_CHECK(Masa1.isApprox(Masa2, 1e-16));
  }
 
  const size_t NBT = 100000;
  std::vector<Eigen::Matrix3d> Sres(NBT);
  std::vector<Eigen::Matrix3d> Rs(NBT);
  for (size_t i = 0; i < NBT; ++i)
    Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double, 4, 1>::Random())).normalized().matrix();
 
  std::cout << "Eigen: ";
  PinocchioTicToc timer(PinocchioTicToc::US);
  timer.tic();
  SMOOTH(NBT)
  {
    timeSelfAdj(Rs[_smooth], M, Sres[_smooth]);
  }
  timer.toc(std::cout, NBT);
}
 
BOOST_AUTO_TEST_CASE(comparison)
{
  using namespace pinocchio;
  Symmetric3 sym1(Symmetric3::Random());
 
  Symmetric3 sym2(sym1);
  sym2.data() *= 2;
 
  BOOST_CHECK(sym2 != sym1);
  BOOST_CHECK(sym1 == sym1);
}
 
BOOST_AUTO_TEST_CASE(cast)
{
  using namespace pinocchio;
  Symmetric3 sym(Symmetric3::Random());
 
  BOOST_CHECK(sym.cast<double>() == sym);
  BOOST_CHECK(sym.cast<long double>().cast<double>() == sym);
}
BOOST_AUTO_TEST_SUITE_END()