Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/symmetric.cpp	Lines:	143	143	100.0 %
Date:	2024-04-26 13:14:21	Branches:	588	1162	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>

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