GCC Code Coverage Report

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