Debian Codegraph @

unittests/unit/math/test_Geometry.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <iostream>
34
35 #include <gtest/gtest.h>
36
37 #include "dart/math/Geometry.hpp"
38 #include "dart/math/Helpers.hpp"
39
40 #include "TestHelpers.hpp"
41
42 using namespace dart;
43 using namespace math;
44
45 #define LIE_GROUP_OPT_TOL 1e-12
46
47 /******************************************************************************/
48 Eigen::Matrix4d toMatrixForm(const Eigen::Vector6d& v)
49 {
50 Eigen::Matrix4d result = Eigen::Matrix4d::Zero();
51
52 result(0, 1) = -v(2);
53 result(1, 0) = v(2);
54 result(0, 2) = v(1);
55 result(2, 0) = -v(1);
56 result(1, 2) = -v(0);
57 result(2, 1) = v(0);
58
59 result(0, 3) = v(3);
60 result(1, 3) = v(4);
61 result(2, 3) = v(5);
62
63 return result;
64 }
65
66 /******************************************************************************/
67 Eigen::Vector6d fromMatrixForm(const Eigen::Matrix4d& m)
68 {
69 Eigen::Vector6d ret;
70 ret << m(2, 1), m(0, 2), m(1, 0), m(0, 3), m(1, 3), m(2, 3);
71 return ret;
72 }
73
74 /******************************************************************************/
75 void testEulerAngles(const Eigen::Vector3d& angle)
76 {
77 Eigen::Matrix3d mat1;
78 Eigen::Matrix3d mat2;
79
80 // XYX
81 mat1 = math::eulerXYXToMatrix(angle);
82 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitX())
83 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitY())
84 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitX());
85
86 EXPECT_TRUE(math::verifyRotation(mat1));
87 EXPECT_TRUE(math::verifyRotation(mat2));
88 EXPECT_TRUE(equals(mat1, mat2));
89 EXPECT_TRUE(equals(mat1, eulerXYXToMatrix(matrixToEulerXYX(mat1))));
90
91 // XYZ
92 mat1 = math::eulerXYZToMatrix(angle);
93 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitX())
94 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitY())
95 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitZ());
96
97 EXPECT_TRUE(math::verifyRotation(mat1));
98 EXPECT_TRUE(math::verifyRotation(mat2));
99 EXPECT_TRUE(equals(mat1, mat2));
100 EXPECT_TRUE(equals(mat1, eulerXYZToMatrix(matrixToEulerXYZ(mat1))));
101
102 // XZX
103 mat1 = math::eulerXZXToMatrix(angle);
104 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitX())
105 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitZ())
106 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitX());
107
108 EXPECT_TRUE(math::verifyRotation(mat1));
109 EXPECT_TRUE(math::verifyRotation(mat2));
110 EXPECT_TRUE(equals(mat1, mat2));
111 // EXPECT_TRUE(equals(mat1, eulerXZXToMatrix(matrixToEulerXZX(mat1))));
112
113 // XZY
114 mat1 = math::eulerXZYToMatrix(angle);
115 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitX())
116 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitZ())
117 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitY());
118
119 EXPECT_TRUE(math::verifyRotation(mat1));
120 EXPECT_TRUE(math::verifyRotation(mat2));
121 EXPECT_TRUE(equals(mat1, mat2));
122 EXPECT_TRUE(equals(mat1, eulerXZYToMatrix(matrixToEulerXZY(mat1))));
123
124 // YXY
125 mat1 = math::eulerYXYToMatrix(angle);
126 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitY())
127 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitX())
128 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitY());
129
130 EXPECT_TRUE(math::verifyRotation(mat1));
131 EXPECT_TRUE(math::verifyRotation(mat2));
132 EXPECT_TRUE(equals(mat1, mat2));
133 // EXPECT_TRUE(equals(mat1, eulerYXYToMatrix(matrixToEulerYXY(mat1))));
134
135 // YXZ
136 mat1 = math::eulerYXZToMatrix(angle);
137 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitY())
138 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitX())
139 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitZ());
140
141 EXPECT_TRUE(math::verifyRotation(mat1));
142 EXPECT_TRUE(math::verifyRotation(mat2));
143 EXPECT_TRUE(equals(mat1, mat2));
144 EXPECT_TRUE(equals(mat1, eulerYXZToMatrix(matrixToEulerYXZ(mat1))));
145
146 // YZX
147 mat1 = math::eulerYZXToMatrix(angle);
148 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitY())
149 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitZ())
150 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitX());
151
152 EXPECT_TRUE(math::verifyRotation(mat1));
153 EXPECT_TRUE(math::verifyRotation(mat2));
154 EXPECT_TRUE(equals(mat1, mat2));
155 EXPECT_TRUE(equals(mat1, eulerYZXToMatrix(matrixToEulerYZX(mat1))));
156
157 // YZY
158 mat1 = math::eulerYZYToMatrix(angle);
159 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitY())
160 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitZ())
161 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitY());
162
163 EXPECT_TRUE(math::verifyRotation(mat1));
164 EXPECT_TRUE(math::verifyRotation(mat2));
165 EXPECT_TRUE(equals(mat1, mat2));
166 // EXPECT_TRUE(equals(mat1, eulerYZYToMatrix(matrixToEulerYZY(mat1))));
167
168 // ZXY
169 mat1 = math::eulerZXYToMatrix(angle);
170 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitZ())
171 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitX())
172 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitY());
173
174 EXPECT_TRUE(math::verifyRotation(mat1));
175 EXPECT_TRUE(math::verifyRotation(mat2));
176 EXPECT_TRUE(equals(mat1, mat2));
177 EXPECT_TRUE(equals(mat1, eulerZXYToMatrix(matrixToEulerZXY(mat1))));
178
179 // ZYX
180 mat1 = math::eulerZYXToMatrix(angle);
181 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitZ())
182 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitY())
183 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitX());
184
185 EXPECT_TRUE(math::verifyRotation(mat1));
186 EXPECT_TRUE(math::verifyRotation(mat2));
187 EXPECT_TRUE(equals(mat1, mat2));
188 EXPECT_TRUE(equals(mat1, eulerZYXToMatrix(matrixToEulerZYX(mat1))));
189
190 // ZXZ
191 mat1 = math::eulerZXZToMatrix(angle);
192 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitZ())
193 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitX())
194 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitZ());
195
196 EXPECT_TRUE(math::verifyRotation(mat1));
197 EXPECT_TRUE(math::verifyRotation(mat2));
198 EXPECT_TRUE(equals(mat1, mat2));
199 // EXPECT_TRUE(equals(mat1, eulerZXZToMatrix(matrixToEulerZXZ(mat1))));
200
201 // ZYZ
202 mat1 = math::eulerZYZToMatrix(angle);
203 mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitZ())
204 * Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitY())
205 * Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitZ());
206
207 EXPECT_TRUE(math::verifyRotation(mat1));
208 EXPECT_TRUE(math::verifyRotation(mat2));
209 EXPECT_TRUE(equals(mat1, mat2));
210 // EXPECT_TRUE(equals(mat1, eulerZYZToMatrix(matrixToEulerZYZ(mat1))));
211 }
212
213 /******************************************************************************/
214 TEST(LIE_GROUP_OPERATORS, EULER_ANGLES)
215 {
216 // TODO: Special angles such as (PI, 0, 0)
217
218 //
219 int numTest = 1;
220 for (int i = 0; i < numTest; ++i)
221 {
222 Eigen::Vector3d angle = Eigen::Vector3d::Random();
223 testEulerAngles(angle);
224 }
225 }
226
227 /******************************************************************************/
228 #define EPSILON_EXPMAP_THETA 1.0e-3
229 TEST(LIE_GROUP_OPERATORS, EXPONENTIAL_MAPPINGS)
230 {
231 int numTest = 100;
232
233 // Exp
234 for (int i = 0; i < numTest; ++i)
235 {
236 Eigen::Vector6d s = Eigen::Vector6d::Random();
237 Eigen::Isometry3d Exp_s = math::expMap(s);
238 Eigen::Matrix4d Exp_s_2 = Eigen::Matrix4d::Identity();
239
240 double theta = s.head<3>().norm();
241 Eigen::Matrix3d R = Matrix3d::Zero();
242 Eigen::Matrix3d qss = math::makeSkewSymmetric(s.head<3>());
243 Eigen::Matrix3d qss2 = qss * qss;
244 Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
245
246 if (theta < EPSILON_EXPMAP_THETA)
247 {
248 R = Matrix3d::Identity() + qss + 0.5 * qss2;
249 P = Matrix3d::Identity() + 0.5 * qss + (1 / 6) * qss2;
250 }
251 else
252 {
253 R = Matrix3d::Identity() + (sin(theta) / theta) * qss
254 + ((1 - cos(theta)) / (theta * theta)) * qss2;
255 P = Matrix3d::Identity() + ((1 - cos(theta)) / (theta * theta)) * qss
256 + ((theta - sin(theta)) / (theta * theta * theta)) * qss2;
257 }
258
259 Exp_s_2.topLeftCorner<3, 3>() = R;
260 Exp_s_2.topRightCorner<3, 1>() = P * s.tail<3>();
261
262 //
263 for (int i = 0; i < 4; ++i)
264 for (int j = 0; j < 4; ++j)
265 EXPECT_NEAR(Exp_s(i, j), Exp_s_2(i, j), LIE_GROUP_OPT_TOL);
266 }
267
268 // ExpAngular
269 for (int i = 0; i < numTest; ++i)
270 {
271 Eigen::Vector6d s = Eigen::Vector6d::Random();
272 s.tail<3>() = Eigen::Vector3d::Zero();
273 Eigen::Isometry3d Exp_s = math::expAngular(s.head<3>());
274 Eigen::Matrix4d Exp_s_2 = Eigen::Matrix4d::Identity();
275
276 double theta = s.head<3>().norm();
277 Eigen::Matrix3d R = Matrix3d::Zero();
278 Eigen::Matrix3d qss = math::makeSkewSymmetric(s.head<3>());
279 Eigen::Matrix3d qss2 = qss * qss;
280 Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
281
282 if (theta < EPSILON_EXPMAP_THETA)
283 {
284 R = Matrix3d::Identity() + qss + 0.5 * qss2;
285 P = Matrix3d::Identity() + 0.5 * qss + (1 / 6) * qss2;
286 }
287 else
288 {
289 R = Matrix3d::Identity() + (sin(theta) / theta) * qss
290 + ((1 - cos(theta)) / (theta * theta)) * qss2;
291 P = Matrix3d::Identity() + ((1 - cos(theta)) / (theta * theta)) * qss
292 + ((theta - sin(theta)) / (theta * theta * theta)) * qss2;
293 }
294
295 Exp_s_2.topLeftCorner<3, 3>() = R;
296 Exp_s_2.topRightCorner<3, 1>() = P * s.tail<3>();
297
298 //
299 for (int i = 0; i < 4; ++i)
300 for (int j = 0; j < 4; ++j)
301 EXPECT_NEAR(Exp_s(i, j), Exp_s_2(i, j), LIE_GROUP_OPT_TOL);
302 }
303
304 // ExpLinear
305 for (int i = 0; i < numTest; ++i)
306 {
307 Eigen::Vector6d s = Eigen::Vector6d::Random();
308 s.head<3>() = Eigen::Vector3d::Zero();
309 Eigen::Isometry3d Exp_s(Eigen::Translation3d(s.tail<3>()));
310 Eigen::Matrix4d Exp_s_2 = Eigen::Matrix4d::Identity();
311
312 double theta = s.head<3>().norm();
313 Eigen::Matrix3d R = Matrix3d::Zero();
314 Eigen::Matrix3d qss = math::makeSkewSymmetric(s.head<3>());
315 Eigen::Matrix3d qss2 = qss * qss;
316 Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
317
318 if (theta < EPSILON_EXPMAP_THETA)
319 {
320 R = Matrix3d::Identity() + qss + 0.5 * qss2;
321 P = Matrix3d::Identity() + 0.5 * qss + (1 / 6) * qss2;
322 }
323 else
324 {
325 R = Matrix3d::Identity() + (sin(theta) / theta) * qss
326 + ((1 - cos(theta)) / (theta * theta)) * qss2;
327 P = Matrix3d::Identity() + ((1 - cos(theta)) / (theta * theta)) * qss
328 + ((theta - sin(theta)) / (theta * theta * theta)) * qss2;
329 }
330
331 Exp_s_2.topLeftCorner<3, 3>() = R;
332 Exp_s_2.topRightCorner<3, 1>() = P * s.tail<3>();
333
334 //
335 for (int i = 0; i < 4; ++i)
336 for (int j = 0; j < 4; ++j)
337 EXPECT_NEAR(Exp_s(i, j), Exp_s_2(i, j), LIE_GROUP_OPT_TOL);
338 }
339 // Exponential mapping test with high values
340 int numExpTests = 100;
341 double min = -1e+128;
342 double max = +1e+128;
343
344 for (int idxTest = 0; idxTest < numExpTests; ++idxTest)
345 {
346 Eigen::Vector3d randomS = Eigen::Vector3d::Zero();
347
348 for (int i = 0; i < 3; ++i)
349 randomS[i] = Random::uniform(min, max);
350
351 Eigen::Isometry3d T = math::expAngular(randomS);
352 EXPECT_TRUE(math::verifyTransform(T));
353 }
354
355 for (int idxTest = 0; idxTest < numExpTests; ++idxTest)
356 {
357 Eigen::Vector6d randomS = Eigen::Vector6d::Zero();
358
359 for (int i = 0; i < 6; ++i)
360 randomS[i] = Random::uniform(min, max);
361
362 Eigen::Isometry3d T = math::expMap(randomS);
363 EXPECT_TRUE(math::verifyTransform(T));
364 }
365 }
366
367 /******************************************************************************/
368 TEST(LIE_GROUP_OPERATORS, ADJOINT_MAPPINGS)
369 {
370 int numTest = 100;
371
372 // AdT(V) == T * V * InvT
373 for (int i = 0; i < numTest; ++i)
374 {
375 Eigen::Vector6d t = Eigen::Vector6d::Random();
376 Eigen::Isometry3d T = math::expMap(t);
377 Eigen::Vector6d V = Eigen::Vector6d::Random();
378
379 Eigen::Vector6d AdTV = AdT(T, V);
380
381 // Ad(T, V) = T * [V] * InvT
382 Eigen::Matrix4d T_V_InvT
383 = T.matrix() * toMatrixForm(V) * T.inverse().matrix();
384 Eigen::Vector6d T_V_InvT_se3 = fromMatrixForm(T_V_InvT);
385
386 for (int j = 0; j < 6; ++j)
387 EXPECT_NEAR(AdTV(j), T_V_InvT_se3(j), LIE_GROUP_OPT_TOL);
388
389 // Ad(T, V) = [R 0; [p]R R] * V
390 Eigen::Matrix6d AdTMatrix = Eigen::Matrix6d::Zero();
391 AdTMatrix.topLeftCorner<3, 3>() = T.linear();
392 AdTMatrix.bottomRightCorner<3, 3>() = T.linear();
393 AdTMatrix.bottomLeftCorner<3, 3>()
394 = math::makeSkewSymmetric(T.translation()) * T.linear();
395 Eigen::Vector6d AdTMatrix_V = AdTMatrix * V;
396 for (int j = 0; j < 6; ++j)
397 EXPECT_NEAR(AdTV(j), AdTMatrix_V(j), LIE_GROUP_OPT_TOL);
398 }
399
400 // AdR == AdT([R 0; 0 1], V)
401 for (int i = 0; i < numTest; ++i)
402 {
403 Eigen::Vector6d t = Eigen::Vector6d::Random();
404 Eigen::Isometry3d T = math::expMap(t);
405 Eigen::Isometry3d R = Eigen::Isometry3d::Identity();
406 R.linear() = T.linear();
407 Eigen::Vector6d V = Eigen::Vector6d::Random();
408
409 Eigen::Vector6d AdTV = AdT(R, V);
410 Eigen::Vector6d AdRV = AdR(T, V);
411
412 for (int j = 0; j < 6; ++j)
413 EXPECT_NEAR(AdTV(j), AdRV(j), LIE_GROUP_OPT_TOL);
414 }
415
416 // AdTAngular == AdT(T, se3(w, 0))
417 for (int i = 0; i < numTest; ++i)
418 {
419 Eigen::Vector6d t = Eigen::Vector6d::Random();
420 Eigen::Isometry3d T = math::expMap(t);
421 Eigen::Vector3d w = Eigen::Vector3d::Random();
422 Eigen::Vector6d V = Eigen::Vector6d::Zero();
423 V.head<3>() = w;
424
425 Eigen::Vector6d AdTV = AdT(T, V);
426 Eigen::Vector6d AdTAng = AdTAngular(T, w);
427
428 for (int j = 0; j < 6; ++j)
429 EXPECT_NEAR(AdTV(j), AdTAng(j), LIE_GROUP_OPT_TOL);
430 }
431
432 // AdTLinear == AdT(T, se3(w, 0))
433 for (int i = 0; i < numTest; ++i)
434 {
435 Eigen::Vector6d t = Eigen::Vector6d::Random();
436 Eigen::Isometry3d T = math::expMap(t);
437 Eigen::Vector3d v = Eigen::Vector3d::Random();
438 Eigen::Vector6d V = Eigen::Vector6d::Zero();
439 V.tail<3>() = v;
440
441 Eigen::Vector6d AdTV = AdT(T, V);
442 Eigen::Vector6d AdTLin = AdTLinear(T, v);
443
444 for (int j = 0; j < 6; ++j)
445 EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL);
446 }
447
448 // AdTJac
449 for (int i = 0; i < numTest; ++i)
450 {
451 Eigen::Vector6d t = Eigen::Vector6d::Random();
452 Eigen::Isometry3d T = math::expMap(t);
453 Eigen::Vector3d v = Eigen::Vector3d::Random();
454 Eigen::Vector6d V = Eigen::Vector6d::Zero();
455 V.tail<3>() = v;
456
457 Eigen::Vector6d AdTV = AdT(T, V);
458 Eigen::Vector6d AdTLin = AdTLinear(T, v);
459
460 for (int j = 0; j < 6; ++j)
461 EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL);
462 }
463
464 // AdInvT
465 for (int i = 0; i < numTest; ++i)
466 {
467 Eigen::Vector6d t = Eigen::Vector6d::Random();
468 Eigen::Isometry3d T = math::expMap(t);
469 Eigen::Isometry3d InvT = T.inverse();
470 Eigen::Vector6d V = Eigen::Vector6d::Random();
471
472 Eigen::Vector6d Ad_InvT = AdT(InvT, V);
473 Eigen::Vector6d AdInv_T = AdInvT(T, V);
474
475 for (int j = 0; j < 6; ++j)
476 EXPECT_NEAR(Ad_InvT(j), AdInv_T(j), LIE_GROUP_OPT_TOL);
477 }
478
479 // AdInvRLinear
480 for (int i = 0; i < numTest; ++i)
481 {
482 Eigen::Vector6d t = Eigen::Vector6d::Random();
483 Eigen::Isometry3d T = math::expMap(t);
484 Eigen::Vector3d v = Eigen::Vector3d::Random();
485 Eigen::Vector6d V = Eigen::Vector6d::Zero();
486 V.tail<3>() = v;
487 Eigen::Isometry3d R = Eigen::Isometry3d::Identity();
488 R.linear() = T.linear();
489
490 Eigen::Vector6d AdT_ = AdT(R.inverse(), V);
491 Eigen::Vector6d AdInvRLinear_ = AdInvRLinear(T, v);
492
493 for (int j = 0; j < 6; ++j)
494 EXPECT_NEAR(AdT_(j), AdInvRLinear_(j), LIE_GROUP_OPT_TOL);
495 }
496
497 // dAdT
498 for (int i = 0; i < numTest; ++i)
499 {
500 Eigen::Vector6d t = Eigen::Vector6d::Random();
501 Eigen::Isometry3d T = math::expMap(t);
502 Eigen::Vector6d F = Eigen::Vector6d::Random();
503
504 Eigen::Vector6d dAdTF = dAdT(T, F);
505
506 // dAd(T, F) = [R 0; [p]R R]^T * F
507 Eigen::Matrix6d AdTMatrix = Eigen::Matrix6d::Zero();
508 AdTMatrix.topLeftCorner<3, 3>() = T.linear();
509 AdTMatrix.bottomRightCorner<3, 3>() = T.linear();
510 AdTMatrix.bottomLeftCorner<3, 3>()
511 = math::makeSkewSymmetric(T.translation()) * T.linear();
512 Eigen::Vector6d AdTTransMatrix_V = AdTMatrix.transpose() * F;
513 for (int j = 0; j < 6; ++j)
514 EXPECT_NEAR(dAdTF(j), AdTTransMatrix_V(j), LIE_GROUP_OPT_TOL);
515 }
516
517 // dAdInvT
518 for (int i = 0; i < numTest; ++i)
519 {
520 Eigen::Vector6d t = Eigen::Vector6d::Random();
521 Eigen::Isometry3d T = math::expMap(t);
522 Eigen::Isometry3d InvT = T.inverse();
523 Eigen::Vector6d F = Eigen::Vector6d::Random();
524
525 Eigen::Vector6d dAdInvT_F = dAdInvT(T, F);
526
527 //
528 Eigen::Vector6d dAd_InvTF = dAdT(InvT, F);
529
530 for (int j = 0; j < 6; ++j)
531 EXPECT_NEAR(dAdInvT_F(j), dAd_InvTF(j), LIE_GROUP_OPT_TOL);
532
533 // dAd(T, F) = [R 0; [p]R R]^T * F
534 Eigen::Matrix6d AdInvTMatrix = Eigen::Matrix6d::Zero();
535 AdInvTMatrix.topLeftCorner<3, 3>() = InvT.linear();
536 AdInvTMatrix.bottomRightCorner<3, 3>() = InvT.linear();
537 AdInvTMatrix.bottomLeftCorner<3, 3>()
538 = math::makeSkewSymmetric(InvT.translation()) * InvT.linear();
539 Eigen::Vector6d AdInvTTransMatrix_V = AdInvTMatrix.transpose() * F;
540 for (int j = 0; j < 6; ++j)
541 EXPECT_NEAR(dAdInvT_F(j), AdInvTTransMatrix_V(j), LIE_GROUP_OPT_TOL);
542 }
543
544 // dAdInvR
545 for (int i = 0; i < numTest; ++i)
546 {
547 Eigen::Vector6d t = Eigen::Vector6d::Random();
548 Eigen::Isometry3d T = math::expMap(t);
549 Eigen::Isometry3d InvT = T.inverse();
550 Eigen::Isometry3d InvR = Eigen::Isometry3d::Identity();
551 InvR.linear() = InvT.linear();
552 Eigen::Vector6d F = Eigen::Vector6d::Random();
553
554 Eigen::Vector6d dAdInvR_F = dAdInvR(T, F);
555
556 //
557 Eigen::Vector6d dAd_InvTF = dAdT(InvR, F);
558
559 for (int j = 0; j < 6; ++j)
560 EXPECT_NEAR(dAdInvR_F(j), dAd_InvTF(j), LIE_GROUP_OPT_TOL);
561 }
562
563 // ad
564 for (int i = 0; i < numTest; ++i)
565 {
566 Eigen::Vector6d V = Eigen::Vector6d::Random();
567 Eigen::Vector6d W = Eigen::Vector6d::Random();
568
569 Eigen::Vector6d ad_V_W = ad(V, W);
570
571 //
572 Eigen::Matrix6d adV_Matrix = Eigen::Matrix6d::Zero();
573 adV_Matrix.topLeftCorner<3, 3>() = math::makeSkewSymmetric(V.head<3>());
574 adV_Matrix.bottomRightCorner<3, 3>() = math::makeSkewSymmetric(V.head<3>());
575 adV_Matrix.bottomLeftCorner<3, 3>() = math::makeSkewSymmetric(V.tail<3>());
576 Eigen::Vector6d adV_Matrix_W = adV_Matrix * W;
577
578 for (int j = 0; j < 6; ++j)
579 EXPECT_NEAR(ad_V_W(j), adV_Matrix_W(j), LIE_GROUP_OPT_TOL);
580 }
581
582 // dad
583 for (int i = 0; i < numTest; ++i)
584 {
585 Eigen::Vector6d V = Eigen::Vector6d::Random();
586 Eigen::Vector6d F = Eigen::Vector6d::Random();
587
588 Eigen::Vector6d dad_V_F = dad(V, F);
589
590 //
591 Eigen::Matrix6d dadV_Matrix = Eigen::Matrix6d::Zero();
592 dadV_Matrix.topLeftCorner<3, 3>() = math::makeSkewSymmetric(V.head<3>());
593 dadV_Matrix.bottomRightCorner<3, 3>()
594 = math::makeSkewSymmetric(V.head<3>());
595 dadV_Matrix.bottomLeftCorner<3, 3>() = math::makeSkewSymmetric(V.tail<3>());
596 Eigen::Vector6d dadV_Matrix_F = dadV_Matrix.transpose() * F;
597
598 for (int j = 0; j < 6; ++j)
599 EXPECT_NEAR(dad_V_F(j), dadV_Matrix_F(j), LIE_GROUP_OPT_TOL);
600 }
601 }