symmetric.cpp
Go to the documentation of this file.
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 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d)
35 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pinocchio::Symmetric3)
36 
37 void timeSym3(const pinocchio::Symmetric3 & S,
38  const pinocchio::Symmetric3::Matrix3 & R,
39  pinocchio::Symmetric3 & res)
40 {
41  res = S.rotate(R);
42 }
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 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  Sym3 S(Sdense);
67  ASA.triangularView<Eigen::Upper>()
68  = A * S * A.transpose();
69 }
70 
71 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
72 
73 /* --- PINOCCHIO ------------------------------------------------------------ */
74 /* --- PINOCCHIO ------------------------------------------------------------ */
75 /* --- PINOCCHIO ------------------------------------------------------------ */
76 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  Matrix3 M = Matrix3::Random(); M = M*M.transpose();
86  Symmetric3 S(M);
87  BOOST_CHECK(S.matrix().isApprox(M, 1e-12));
88  }
89 
90  // S += S
91  {
92  Symmetric3
93  S = Symmetric3::Random(),
94  S2 = Symmetric3::Random();
95  Symmetric3 Scopy = S;
96  S+=S2;
97  BOOST_CHECK(S.matrix().isApprox(S2.matrix()+Scopy.matrix(), 1e-12));
98  }
99 
100  // S + M
101  {
102  Symmetric3 S = Symmetric3::Random();
103  Matrix3 M = Matrix3::Random(); M = M*M.transpose();
104 
105  Symmetric3 S2 = S + M;
106  BOOST_CHECK(S2.matrix().isApprox(S.matrix()+M, 1e-12));
107 
108  S2 = S - M;
109  BOOST_CHECK(S2.matrix().isApprox(S.matrix()-M, 1e-12));
110  }
111 
112  // S*v
113  {
114  Symmetric3 S = Symmetric3::Random();
115  Vector3 v = Vector3::Random();
116  Vector3 Sv = S*v;
117  BOOST_CHECK(Sv.isApprox(S.matrix()*v, 1e-12));
118  }
119 
120  // Random
121  for(int i=0;i<100;++i )
122  {
123  Matrix3 M = Matrix3::Random(); M = M*M.transpose();
124  Symmetric3 S = Symmetric3::RandomPositive();
125  Vector3 v = Vector3::Random();
126  BOOST_CHECK_GT( (v.transpose()*(S*v))[0] , 0);
127  }
128 
129  // Identity
130  {
131  BOOST_CHECK(Symmetric3::Identity().matrix().isApprox(Matrix3::Identity(), 1e-12));
132  }
133 
134  // Skew2
135  {
136  Vector3 v = Vector3::Random();
137  Symmetric3 vxvx = Symmetric3::SkewSquare(v);
138 
139  Vector3 p = Vector3::UnitX();
140  BOOST_CHECK((vxvx*p).isApprox(v.cross(v.cross(p)), 1e-12));
141 
142  p = Vector3::UnitY();
143  BOOST_CHECK((vxvx*p).isApprox(v.cross(v.cross(p)), 1e-12));
144 
145  p = Vector3::UnitZ();
146  BOOST_CHECK((vxvx*p).isApprox(v.cross(v.cross(p)), 1e-12));
147 
148  Matrix3 vx = skew(v);
149  Matrix3 vxvx2 = (vx*vx).eval();
150  BOOST_CHECK(vxvx.matrix().isApprox(vxvx2, 1e-12));
151 
152  Symmetric3 S = Symmetric3::RandomPositive();
153  BOOST_CHECK((S-Symmetric3::SkewSquare(v)).matrix()
154  .isApprox(S.matrix()-vxvx2, 1e-12));
155 
156  double m = Eigen::internal::random<double>()+1;
157  BOOST_CHECK((S-m*Symmetric3::SkewSquare(v)).matrix()
158  .isApprox(S.matrix()-m*vxvx2, 1e-12));
159 
160 
161  Symmetric3 S2 = S;
162  S -= Symmetric3::SkewSquare(v);
163  BOOST_CHECK(S.matrix().isApprox(S2.matrix()-vxvx2, 1e-12));
164 
165  S = S2; S -= m*Symmetric3::SkewSquare(v);
166  BOOST_CHECK(S.matrix().isApprox(S2.matrix()-m*vxvx2, 1e-12));
167 
168  }
169 
170  // (i,j)
171  {
172  Matrix3 M = Matrix3::Random(); M = M*M.transpose();
173  Symmetric3 S(M);
174  for(int i=0;i<3;++i)
175  for(int j=0;j<3;++j)
176  BOOST_CHECK_SMALL(S(i,j) - M(i,j), Eigen::NumTraits<double>::dummy_precision());
177  }
178  }
179 
180  // SRS
181  {
182  Symmetric3 S = Symmetric3::RandomPositive();
183  Matrix3 R = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
184 
185  Symmetric3 RSRt = S.rotate(R);
186  BOOST_CHECK(RSRt.matrix().isApprox(R*S.matrix()*R.transpose(), 1e-12));
187 
188  Symmetric3 RtSR = S.rotate(R.transpose());
189  BOOST_CHECK(RtSR.matrix().isApprox(R.transpose()*S.matrix()*R, 1e-12));
190  }
191 
192  // Test operator vtiv
193  {
194  Symmetric3 S = Symmetric3::RandomPositive();
195  Vector3 v = Vector3::Random();
196  double kinetic_ref = v.transpose() * S.matrix() * v;
197  double kinetic = S.vtiv(v);
198  BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
199  }
200 
201  // Test v x S3
202  {
203  Symmetric3 S = Symmetric3::RandomPositive();
204  Vector3 v = Vector3::Random();
205  Matrix3 Vcross = skew(v);
206  Matrix3 M_ref(Vcross * S.matrix());
207 
208  Matrix3 M_res;
209  Symmetric3::vxs(v,S,M_res);
210  BOOST_CHECK(M_res.isApprox(M_ref));
211 
212  BOOST_CHECK(S.vxs(v).isApprox(M_ref));
213  }
214 
215  // Test S3 vx
216  {
217  Symmetric3 S = Symmetric3::RandomPositive();
218  Vector3 v = Vector3::Random();
219  Matrix3 Vcross = skew(v);
220  Matrix3 M_ref(S.matrix() * Vcross);
221 
222  Matrix3 M_res;
223  Symmetric3::svx(v,S,M_res);
224  BOOST_CHECK(M_res.isApprox(M_ref));
225 
226  BOOST_CHECK(S.svx(v).isApprox(M_ref));
227  }
228 
229  // Test isZero
230  {
231  Symmetric3 S_not_zero = Symmetric3::Identity();
232  BOOST_CHECK(!S_not_zero.isZero());
233 
234  Symmetric3 S_zero = Symmetric3::Zero();
235  BOOST_CHECK(S_zero.isZero());
236  }
237 
238  // Test isApprox
239  {
240  Symmetric3 S1 = Symmetric3::RandomPositive();
241  Symmetric3 S2 = S1;
242 
243  BOOST_CHECK(S1.isApprox(S2));
244 
245  Symmetric3 S3 = S1;
246  S3 += S3;
247  BOOST_CHECK(!S1.isApprox(S3));
248  }
249 
250  // Time test
251  {
252  const size_t NBT = 100000;
253  Symmetric3 S = Symmetric3::RandomPositive();
254 
255  std::vector<Symmetric3> Sres (NBT);
256  std::vector<Matrix3> Rs (NBT);
257  for(size_t i=0;i<NBT;++i)
258  Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
259 
260  std::cout << "Pinocchio: ";
261  PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
262  SMOOTH(NBT)
263  {
264  timeSym3(S,Rs[_smooth],Sres[_smooth]);
265  }
266  timer.toc(std::cout,NBT);
267  }
268 }
269 
270 /* --- EIGEN SYMMETRIC ------------------------------------------------------ */
271 /* --- EIGEN SYMMETRIC ------------------------------------------------------ */
272 /* --- EIGEN SYMMETRIC ------------------------------------------------------ */
273 
274 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  Matrix3 M = Matrix3::Random();
281  Sym3 S(M);
282  {
283  Matrix3 Scp = S;
284  BOOST_CHECK((Scp-Scp.transpose()).isApprox(Matrix3::Zero(), 1e-16));
285  }
286 
287  Matrix3 M2 = Matrix3::Random();
288  M.triangularView<Eigen::Upper>() = M2;
289 
290  Matrix3 A = Matrix3::Random(), ASA1, ASA2;
291  ASA1.triangularView<Eigen::Upper>() = A * S * A.transpose();
292  timeSelfAdj(A,M,ASA2);
293 
294  {
295  Matrix3 Masa1 = ASA1.selfadjointView<Eigen::Upper>();
296  Matrix3 Masa2 = ASA2.selfadjointView<Eigen::Upper>();
297  BOOST_CHECK(Masa1.isApprox(Masa2, 1e-16));
298  }
299 
300  const size_t NBT = 100000;
301  std::vector<Eigen::Matrix3d> Sres (NBT);
302  std::vector<Eigen::Matrix3d> Rs (NBT);
303  for(size_t i=0;i<NBT;++i)
304  Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
305 
306  std::cout << "Eigen: ";
307  PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
308  SMOOTH(NBT)
309  {
310  timeSelfAdj(Rs[_smooth],M,Sres[_smooth]);
311  }
312  timer.toc(std::cout,NBT);
313 }
314 
315 BOOST_AUTO_TEST_CASE(comparison)
316 {
317  using namespace pinocchio;
318  Symmetric3 sym1(Symmetric3::Random());
319 
320  Symmetric3 sym2(sym1);
321  sym2.data() *= 2;
322 
323  BOOST_CHECK(sym2 != sym1);
324  BOOST_CHECK(sym1 == sym1);
325 }
326 
327 BOOST_AUTO_TEST_CASE(cast)
328 {
329  using namespace pinocchio;
330  Symmetric3 sym(Symmetric3::Random());
331 
332  BOOST_CHECK(sym.cast<double>() == sym);
333  BOOST_CHECK(sym.cast<long double>().cast<double>() == sym);
334 
335 }
336 BOOST_AUTO_TEST_SUITE_END ()
337 
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:746
pinocchio::Symmetric3Tpl::isApprox
bool isApprox(const Symmetric3Tpl &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: spatial/symmetric3.hpp:73
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:68
pinocchio::Symmetric3Tpl< double, 0 >::Matrix3
Eigen::Matrix< Scalar, 3, 3, Options > Matrix3
Definition: spatial/symmetric3.hpp:22
pinocchio::cast
NewScalar cast(const Scalar &value)
Definition: cast.hpp:13
pinocchio::Symmetric3Tpl::isZero
bool isZero(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: spatial/symmetric3.hpp:77
pinocchio::Symmetric3Tpl::cast
Symmetric3Tpl< NewScalar, Options > cast() const
Definition: spatial/symmetric3.hpp:453
PinocchioTicToc::tic
void tic()
Definition: timer.hpp:63
pinocchio::casadi::sym
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
Definition: autodiff/casadi.hpp:170
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
pinocchio::Symmetric3Tpl< double, 0 >::svx
static void svx(const Eigen::MatrixBase< Vector3 > &v, const Symmetric3Tpl &S3, const Eigen::MatrixBase< Matrix3 > &M)
Performs the operation .
Definition: spatial/symmetric3.hpp:284
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
R
R
BOOST_TEST_MODULE
#define BOOST_TEST_MODULE
m
float m
pinocchio::Symmetric3Tpl::rotate
Symmetric3Tpl rotate(const Eigen::MatrixBase< D > &R) const
Definition: spatial/symmetric3.hpp:415
pinocchio::Symmetric3Tpl::vtiv
Scalar vtiv(const Vector3 &v) const
Definition: spatial/symmetric3.hpp:196
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
res
res
pinocchio::Symmetric3Tpl< double, 0 >::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: spatial/symmetric3.hpp:20
pinocchio::Symmetric3Tpl< double, 0 >::vxs
static void vxs(const Eigen::MatrixBase< Vector3 > &v, const Symmetric3Tpl &S3, const Eigen::MatrixBase< Matrix3 > &M)
Performs the operation . This operation is equivalent to applying the cross product of v on each colu...
Definition: spatial/symmetric3.hpp:223
pinocchio::Symmetric3
Symmetric3Tpl< double, 0 > Symmetric3
Definition: src/spatial/fwd.hpp:60
pinocchio::skew
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:21
pinocchio::Symmetric3Tpl::data
const Vector6 & data() const
Definition: spatial/symmetric3.hpp:160
M
M
A
A
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_pinocchio_Sym3)
Definition: symmetric.cpp:76
timeSym3
void timeSym3(const pinocchio::Symmetric3 &S, const pinocchio::Symmetric3::Matrix3 &R, pinocchio::Symmetric3 &res)
Definition: symmetric.cpp:37
timeSelfAdj
void timeSelfAdj(const Eigen::Matrix3d &A, const Eigen::Matrix3d &Sdense, Eigen::Matrix3d &ASA)
Definition: symmetric.cpp:61

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:33