Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/symmetric.cpp	Lines:	145	145	100.0 %
Date:	2023-08-09 08:43:58	Branches:	594	1174	50.6 %


//
// Copyright (c) 2015-2016,2018 CNRS
//

/* --- 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>


#include <Eigen/StdVector>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pinocchio::Symmetric3)

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));
    }

    // 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));
  }

    // 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 ()



Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2016,2018 CNRS
3			//
4
5			/* --- Unitary test symmetric.cpp This code tests and compares two ways of
6			* expressing symmetric matrices. In addition to the unitary validation (test
7			* of the basic operations), the code is validating the computation
8			* performances of each methods.
9			*
10			* The three methods are:
11			* - Eigen SelfAdjoint (a mask atop of a classical dense matrix) ==> the least efficient.
12			* - Pinocchio rewritting of Metapod code with LTI factor as well and minor improvement.
13			*
14			* IMPORTANT: the following timings seems outdated.
15			* Expected time scores on a I7 2.1GHz:
16			* - Eigen: 2.5us
17			* - Pinocchio: 6us
18			*/
19
20			#include "pinocchio/spatial/fwd.hpp"
21			#include "pinocchio/spatial/skew.hpp"
22			#include "pinocchio/utils/timer.hpp"
23
24			#include <boost/random.hpp>
25			#include <Eigen/Geometry>
26
27			#include "pinocchio/spatial/symmetric3.hpp"
28
29			#include <boost/test/unit_test.hpp>
30			#include <boost/utility/binary.hpp>
31
32
33			#include <Eigen/StdVector>
34	✓✗	3	EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d)
35	✓✗	1	EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pinocchio::Symmetric3)
36
37		100000	void timeSym3(const pinocchio::Symmetric3 & S,
38			const pinocchio::Symmetric3::Matrix3 & R,
39			pinocchio::Symmetric3 & res)
40			{
41		100000	res = S.rotate(R);
42		100000	}
43
44			#ifdef WITH_METAPOD
45
46			#include <metapod/tools/spatial/lti.hh>
47			#include <metapod/tools/spatial/rm-general.hh>
48
49			EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::ltI<double>)
50			EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::RotationMatrixTpl<double>)
51
52			void timeLTI(const metapod::Spatial::ltI<double>& S,
53			const metapod::Spatial::RotationMatrixTpl<double>& R,
54			metapod::Spatial::ltI<double> & res)
55			{
56			res = R.rotTSymmetricMatrix(S);
57			}
58
59			#endif
60
61		100001	void timeSelfAdj( const Eigen::Matrix3d & A,
62			const Eigen::Matrix3d & Sdense,
63			Eigen::Matrix3d & ASA )
64			{
65			typedef Eigen::SelfAdjointView<const Eigen::Matrix3d,Eigen::Upper> Sym3;
66	✓✗	100001	Sym3 S(Sdense);
67	✓✗	100001	ASA.triangularView<Eigen::Upper>()
68	✓✗✓✗ ✓✗✓✗	200002	= A * S * A.transpose();
69		100001	}
70
71			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
72
73			/* --- PINOCCHIO ------------------------------------------------------------ */
74			/* --- PINOCCHIO ------------------------------------------------------------ */
75			/* --- PINOCCHIO ------------------------------------------------------------ */
76	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
77			{
78			using namespace pinocchio;
79			typedef Symmetric3::Matrix3 Matrix3;
80			typedef Symmetric3::Vector3 Vector3;
81
82			{
83			// op(Matrix3)
84			{
85	✓✗✓✗ ✓✗✓✗ ✓✗	2	Matrix3 M = Matrix3::Random(); M = M*M.transpose();
86	✓✗	2	Symmetric3 S(M);
87	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(S.matrix().isApprox(M, 1e-12));
88			}
89
90			// S += S
91			{
92			Symmetric3
93	✓✗	2	S = Symmetric3::Random(),
94	✓✗	2	S2 = Symmetric3::Random();
95	✓✗	2	Symmetric3 Scopy = S;
96	✓✗	2	S+=S2;
97	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(S.matrix().isApprox(S2.matrix()+Scopy.matrix(), 1e-12));
98			}
99
100			// S + M
101			{
102	✓✗	2	Symmetric3 S = Symmetric3::Random();
103	✓✗✓✗ ✓✗✓✗ ✓✗	2	Matrix3 M = Matrix3::Random(); M = M*M.transpose();
104
105	✓✗	2	Symmetric3 S2 = S + M;
106	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(S2.matrix().isApprox(S.matrix()+M, 1e-12));
107
108	✓✗	2	S2 = S - M;
109	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(S2.matrix().isApprox(S.matrix()-M, 1e-12));
110			}
111
112			// S*v
113			{
114	✓✗	2	Symmetric3 S = Symmetric3::Random();
115	✓✗✓✗	2	Vector3 v = Vector3::Random();
116	✓✗	2	Vector3 Sv = S*v;
117	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(Sv.isApprox(S.matrix()*v, 1e-12));
118			}
119
120			// Random
121	✓✓	202	for(int i=0;i<100;++i )
122			{
123	✓✗✓✗ ✓✗✓✗ ✓✗	200	Matrix3 M = Matrix3::Random(); M = M*M.transpose();
124	✓✗	200	Symmetric3 S = Symmetric3::RandomPositive();
125	✓✗✓✗	200	Vector3 v = Vector3::Random();
126	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	200	BOOST_CHECK_GT( (v.transpose()(Sv))[0] , 0);
127			}
128
129			// Identity
130			{
131	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(Symmetric3::Identity().matrix().isApprox(Matrix3::Identity(), 1e-12));
132			}
133
134			// Skew2
135			{
136	✓✗✓✗	2	Vector3 v = Vector3::Random();
137	✓✗	2	Symmetric3 vxvx = Symmetric3::SkewSquare(v);
138
139	✓✗✓✗	2	Vector3 p = Vector3::UnitX();
140	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((vxvx*p).isApprox(v.cross(v.cross(p)), 1e-12));
141
142	✓✗✓✗	2	p = Vector3::UnitY();
143	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((vxvx*p).isApprox(v.cross(v.cross(p)), 1e-12));
144
145	✓✗✓✗	2	p = Vector3::UnitZ();
146	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((vxvx*p).isApprox(v.cross(v.cross(p)), 1e-12));
147
148	✓✗	2	Matrix3 vx = skew(v);
149	✓✗✓✗	2	Matrix3 vxvx2 = (vx*vx).eval();
150	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(vxvx.matrix().isApprox(vxvx2, 1e-12));
151
152	✓✗	2	Symmetric3 S = Symmetric3::RandomPositive();
153	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK((S-Symmetric3::SkewSquare(v)).matrix()
154			.isApprox(S.matrix()-vxvx2, 1e-12));
155
156	✓✗	2	double m = Eigen::internal::random<double>()+1;
157	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK((S-m*Symmetric3::SkewSquare(v)).matrix()
158			.isApprox(S.matrix()-m*vxvx2, 1e-12));
159
160
161	✓✗	2	Symmetric3 S2 = S;
162	✓✗	2	S -= Symmetric3::SkewSquare(v);
163	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(S.matrix().isApprox(S2.matrix()-vxvx2, 1e-12));
164
165	✓✗✓✗ ✓✗	2	S = S2; S -= m*Symmetric3::SkewSquare(v);
166	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(S.matrix().isApprox(S2.matrix()-m*vxvx2, 1e-12));
167
168			}
169
170			// (i,j)
171			{
172	✓✗✓✗ ✓✗✓✗ ✓✗	2	Matrix3 M = Matrix3::Random(); M = M*M.transpose();
173	✓✗	2	Symmetric3 S(M);
174	✓✓	8	for(int i=0;i<3;++i)
175	✓✓	24	for(int j=0;j<3;++j)
176	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	18	BOOST_CHECK_SMALL(S(i,j) - M(i,j), Eigen::NumTraits<double>::dummy_precision());
177			}
178			}
179
180			// SRS
181			{
182	✓✗	2	Symmetric3 S = Symmetric3::RandomPositive();
183	✓✗✓✗ ✓✗✓✗	2	Matrix3 R = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
184
185	✓✗	2	Symmetric3 RSRt = S.rotate(R);
186	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(RSRt.matrix().isApprox(RS.matrix()R.transpose(), 1e-12));
187
188	✓✗✓✗	2	Symmetric3 RtSR = S.rotate(R.transpose());
189	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(RtSR.matrix().isApprox(R.transpose()S.matrix()R, 1e-12));
190			}
191
192			// Test operator vtiv
193			{
194	✓✗	2	Symmetric3 S = Symmetric3::RandomPositive();
195	✓✗✓✗	2	Vector3 v = Vector3::Random();
196	✓✗✓✗ ✓✗✓✗ ✓✗	2	double kinetic_ref = v.transpose() * S.matrix() * v;
197	✓✗	2	double kinetic = S.vtiv(v);
198	✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
199			}
200
201			// Test v x S3
202			{
203	✓✗	2	Symmetric3 S = Symmetric3::RandomPositive();
204	✓✗✓✗	2	Vector3 v = Vector3::Random();
205	✓✗	2	Matrix3 Vcross = skew(v);
206	✓✗✓✗ ✓✗	2	Matrix3 M_ref(Vcross * S.matrix());
207
208	✓✗	2	Matrix3 M_res;
209	✓✗	2	Symmetric3::vxs(v,S,M_res);
210	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M_res.isApprox(M_ref));
211
212	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(S.vxs(v).isApprox(M_ref));
213			}
214
215			// Test S3 vx
216			{
217	✓✗	2	Symmetric3 S = Symmetric3::RandomPositive();
218	✓✗✓✗	2	Vector3 v = Vector3::Random();
219	✓✗	2	Matrix3 Vcross = skew(v);
220	✓✗✓✗ ✓✗	2	Matrix3 M_ref(S.matrix() * Vcross);
221
222	✓✗	2	Matrix3 M_res;
223	✓✗	2	Symmetric3::svx(v,S,M_res);
224	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M_res.isApprox(M_ref));
225
226	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(S.svx(v).isApprox(M_ref));
227			}
228
229			// Test isZero
230			{
231	✓✗	2	Symmetric3 S_not_zero = Symmetric3::Identity();
232	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(!S_not_zero.isZero());
233
234	✓✗	2	Symmetric3 S_zero = Symmetric3::Zero();
235	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(S_zero.isZero());
236			}
237
238			// Test isApprox
239			{
240	✓✗	2	Symmetric3 S1 = Symmetric3::RandomPositive();
241	✓✗	2	Symmetric3 S2 = S1;
242
243	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(S1.isApprox(S2));
244
245	✓✗	2	Symmetric3 S3 = S1;
246	✓✗	2	S3 += S3;
247	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(!S1.isApprox(S3));
248			}
249
250			// Time test
251			{
252		2	const size_t NBT = 100000;
253	✓✗	2	Symmetric3 S = Symmetric3::RandomPositive();
254
255	✓✗✓✗	4	std::vector<Symmetric3> Sres (NBT);
256	✓✗✓✗	4	std::vector<Matrix3> Rs (NBT);
257	✓✓	200002	for(size_t i=0;i<NBT;++i)
258	✓✗✓✗ ✓✗✓✗	200000	Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
259
260	✓✗	2	std::cout << "Pinocchio: ";
261	✓✗✓✗	4	PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
262	✓✓	200002	SMOOTH(NBT)
263			{
264	✓✗	200000	timeSym3(S,Rs[_smooth],Sres[_smooth]);
265			}
266	✓✗	2	timer.toc(std::cout,NBT);
267			}
268		2	}
269
270			/* --- EIGEN SYMMETRIC ------------------------------------------------------ */
271			/* --- EIGEN SYMMETRIC ------------------------------------------------------ */
272			/* --- EIGEN SYMMETRIC ------------------------------------------------------ */
273
274	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_eigen_SelfAdj )
275			{
276			using namespace pinocchio;
277			typedef Eigen::Matrix3d Matrix3;
278			typedef Eigen::SelfAdjointView<Matrix3,Eigen::Upper> Sym3;
279
280	✓✗✓✗	2	Matrix3 M = Matrix3::Random();
281	✓✗	2	Sym3 S(M);
282			{
283	✓✗	2	Matrix3 Scp = S;
284	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((Scp-Scp.transpose()).isApprox(Matrix3::Zero(), 1e-16));
285			}
286
287	✓✗✓✗	2	Matrix3 M2 = Matrix3::Random();
288	✓✗✓✗	2	M.triangularView<Eigen::Upper>() = M2;
289
290	✓✗✓✗ ✓✗✓✗	2	Matrix3 A = Matrix3::Random(), ASA1, ASA2;
291	✓✗✓✗ ✓✗✓✗ ✓✗	2	ASA1.triangularView<Eigen::Upper>() = A * S * A.transpose();
292	✓✗	2	timeSelfAdj(A,M,ASA2);
293
294			{
295	✓✗✓✗	2	Matrix3 Masa1 = ASA1.selfadjointView<Eigen::Upper>();
296	✓✗✓✗	2	Matrix3 Masa2 = ASA2.selfadjointView<Eigen::Upper>();
297	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(Masa1.isApprox(Masa2, 1e-16));
298			}
299
300		2	const size_t NBT = 100000;
301	✓✗✓✗	4	std::vector<Eigen::Matrix3d> Sres (NBT);
302	✓✗✓✗	4	std::vector<Eigen::Matrix3d> Rs (NBT);
303	✓✓	200002	for(size_t i=0;i<NBT;++i)
304	✓✗✓✗ ✓✗✓✗	200000	Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
305
306	✓✗	2	std::cout << "Eigen: ";
307	✓✗✓✗	4	PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
308	✓✓	200002	SMOOTH(NBT)
309			{
310	✓✗	200000	timeSelfAdj(Rs[_smooth],M,Sres[_smooth]);
311			}
312	✓✗	2	timer.toc(std::cout,NBT);
313		2	}
314
315	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(comparison)
316			{
317			using namespace pinocchio;
318	✓✗	2	Symmetric3 sym1(Symmetric3::Random());
319
320	✓✗	2	Symmetric3 sym2(sym1);
321	✓✗	2	sym2.data() *= 2;
322
323	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(sym2 != sym1);
324	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(sym1 == sym1);
325		2	}
326
327	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(cast)
328			{
329			using namespace pinocchio;
330	✓✗	2	Symmetric3 sym(Symmetric3::Random());
331
332	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(sym.cast<double>() == sym);
333	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(sym.cast<long double>().cast<double>() == sym);
334
335		2	}
336			BOOST_AUTO_TEST_SUITE_END ()
337