farm-ng-core
lie_group_prop_tests.h
Go to the documentation of this file.
1 // Copyright (c) 2011, Hauke Strasdat
2 // Copyright (c) 2012, Steven Lovegrove
3 // Copyright (c) 2021, farm-ng, inc.
4 //
5 // Use of this source code is governed by an MIT-style
6 // license that can be found in the LICENSE file or at
7 // https://opensource.org/licenses/MIT.
8 
9 #pragma once
10 
11 #include "sophus/calculus/num_diff.h"
12 #include "sophus/concepts/lie_group.h"
13 #include "sophus/linalg/vector_space.h"
14 
15 #include <unsupported/Eigen/MatrixFunctions> // for matrix exp
16 
17 namespace sophus {
18 namespace test {
19 
20 template <concepts::LieGroup TGroup>
21 struct LieGroupPropTestSuite {
22  using Group = TGroup;
23 
24  using Scalar = typename Group::Scalar;
25 
26  static int constexpr kDof = Group::kDof;
27  static int constexpr kNumParams = Group::kNumParams;
28  static int constexpr kPointDim = Group::kPointDim;
29  static int constexpr kAmbientDim = Group::kAmbientDim;
30 
31  static decltype(Group::elementExamples()) const kElementExamples;
32  static decltype(Group::tangentExamples()) const kTangentExamples;
33  static decltype(pointExamples<Scalar, Group::kPointDim>())
34  const kPointExamples;
35 
36  using Tangent = Eigen::Vector<Scalar, kDof>;
37  using Params = Eigen::Vector<Scalar, kNumParams>;
38  using Point = Eigen::Vector<Scalar, kPointDim>;
39  using Matrix = Eigen::Matrix<Scalar, kAmbientDim, kAmbientDim>;
40  using CompactMatrix = Eigen::Matrix<Scalar, kPointDim, kAmbientDim>;
41 
42  static auto preservabilityTests(std::string group_name) -> void {
43  if (kElementExamples.size() == 0) {
44  return;
45  }
46 
47  if (Group::kIsOriginPreserving) {
48  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
49  Group g = SOPHUS_AT(kElementExamples, g_id);
50  Point o;
51  o.setZero();
52  SOPHUS_ASSERT_NEAR(g * o, o, kEpsilon<Scalar>);
53  }
54  } else {
55  size_t num_preserves = 0;
56  size_t num = 0;
57  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
58  Group g = SOPHUS_AT(kElementExamples, g_id);
59  Point o;
60  o.setZero();
61  if ((g * o).norm() < kEpsilon<Scalar>) {
62  ++num_preserves;
63  }
64  ++num;
65  }
66  float percentage = float(num_preserves) / float(num);
67  FARM_ASSERT_LE(percentage, 0.75);
68  }
69 
70  if (Group::kIsAxisDirectionPreserving) {
71  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
72  Group g = SOPHUS_AT(kElementExamples, g_id);
73  for (int d = 0; d < kPointDim; ++d) {
74  Point p;
75  p.setZero();
76  p[d] = 1.0;
77  UnitVector<Scalar, kPointDim> e =
78  UnitVector<Scalar, kPointDim>::fromUnitVector(p);
79  SOPHUS_ASSERT_NEAR((g * e).params(), e.params(), kEpsilon<Scalar>);
80  }
81  }
82  } else {
83  size_t num_preserves = 0;
84  size_t num = 0;
85  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
86  Group g = SOPHUS_AT(kElementExamples, g_id);
87  for (int d = 0; d < kPointDim; ++d) {
88  Point p;
89  p.setZero();
90  p[d] = 1.0;
91  UnitVector<Scalar, kPointDim> e =
92  UnitVector<Scalar, kPointDim>::fromUnitVector(p);
93 
94  if (((g * e).params() - e.params()).norm() < kEpsilon<Scalar>) {
95  ++num_preserves;
96  }
97  ++num;
98  }
99  }
100  float percentage = float(num_preserves) / float(num);
101  FARM_ASSERT_LE(percentage, 0.75);
102  }
103 
104  if (Group::kIsDirectionVectorPreserving) {
105  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
106  Group g = SOPHUS_AT(kElementExamples, g_id);
107  for (size_t point_id = 0; point_id < kPointExamples.size();
108  ++point_id) {
109  auto p = SOPHUS_AT(kPointExamples, point_id);
110  if (p.norm() < kEpsilon<Scalar>) {
111  continue;
112  }
113  UnitVector<Scalar, kPointDim> d =
114  UnitVector<Scalar, kPointDim>::fromVectorAndNormalize(p);
115  SOPHUS_ASSERT_NEAR((g * d).params(), d.params(), kEpsilon<Scalar>);
116  }
117  }
118  } else {
119  size_t num_preserves = 0;
120  size_t num = 0;
121  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
122  Group g = SOPHUS_AT(kElementExamples, g_id);
123  for (size_t point_id = 0; point_id < kPointExamples.size();
124  ++point_id) {
125  auto p = SOPHUS_AT(kPointExamples, point_id);
126  if (p.norm() < kEpsilon<Scalar>) {
127  continue;
128  }
129  UnitVector<Scalar, kPointDim> d =
130  UnitVector<Scalar, kPointDim>::fromVectorAndNormalize(p);
131  if (((g * d).params() - d.params()).norm() < kEpsilon<Scalar>) {
132  ++num_preserves;
133  }
134  ++num;
135  }
136  }
137  float percentage = float(num_preserves) / float(num);
138  FARM_ASSERT_LE(percentage, 0.75);
139  }
140  }
141 
142  static auto expTests(std::string group_name) -> void {
143  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
144  Group g = SOPHUS_AT(kElementExamples, g_id);
145  auto matrix_before = g.compactMatrix();
146  auto matrix_after = Group::exp(g.log()).compactMatrix();
147 
148  SOPHUS_ASSERT_NEAR(
149  matrix_before,
150  matrix_after,
151  kEpsilonSqrt<Scalar>,
152  "`exp(log(g)) == g` Test for {}\n"
153  "params #{}",
154  group_name,
155  g_id);
156  }
157  for (size_t i = 0; i < kTangentExamples.size(); ++i) {
158  Tangent tangent = SOPHUS_AT(kTangentExamples, i);
159 
160  Group exp_inverse = Group::exp(tangent).inverse();
161  Group exp_neg_tangent = Group::exp(-tangent);
162 
163  SOPHUS_ASSERT_NEAR(
164  exp_inverse.compactMatrix(),
165  exp_neg_tangent.compactMatrix(),
166  0.001,
167  "`exp(-t) == inv(exp(t))` Test for {}\n"
168  "Group::exp(tangent): \n{}\ntangent #{} {}",
169  group_name,
170  Group::exp(tangent).compactMatrix(),
171  i,
172  tangent);
173  }
174 
175  for (size_t i = 0; i < kTangentExamples.size(); ++i) {
176  Tangent omega = SOPHUS_AT(kTangentExamples, i);
177  Matrix exp_x = Group::exp(omega).matrix();
178  Matrix expmap_hat_x = (Group::hat(omega)).exp();
179  SOPHUS_ASSERT_NEAR(
180  exp_x, expmap_hat_x, 0.003, "expmap(hat(x)) - exp(x) case: %", i);
181  }
182  }
183 
184  static auto adjointTests(std::string group_name) -> void {
185  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
186  Group g = SOPHUS_AT(kElementExamples, g_id);
187  Matrix mat = g.matrix();
188  Eigen::Matrix<Scalar, Group::kDof, Group::kDof> mat_adj = g.adj();
189  for (size_t tangent_id = 0; tangent_id < kTangentExamples.size();
190  ++tangent_id) {
191  Tangent x = SOPHUS_AT(kTangentExamples, tangent_id);
192  Tangent mat_adj_x = mat_adj * x;
193  Tangent mat_adj_x2 =
194  Group::vee(mat * Group::hat(x) * g.inverse().matrix());
195  SOPHUS_ASSERT_NEAR(
196  mat_adj_x,
197  mat_adj_x2,
198  10 * kEpsilonSqrt<Scalar>,
199  "`Adj * x == vee(g * hat(x) * inv(g))` Test for {}\n"
200  "Adj: {}"
201  "tangent # {} ({})\n"
202  "params # {} ({}); matrix:\n"
203  "{}",
204  group_name,
205  mat_adj,
206  tangent_id,
207  x.transpose(),
208  g_id,
209  g.params().transpose(),
210  g.compactMatrix());
211  }
212  }
213 
214  for (size_t tangent_a_id = 0; tangent_a_id < kTangentExamples.size();
215  ++tangent_a_id) {
216  Tangent a = SOPHUS_AT(kTangentExamples, tangent_a_id);
217 
218  for (size_t tangent_b_id = 0; tangent_b_id < kTangentExamples.size();
219  ++tangent_b_id) {
220  Tangent b = SOPHUS_AT(kTangentExamples, tangent_b_id);
221 
222  Eigen::Matrix<Scalar, kDof, kDof> ad_a = Group::ad(a);
223  Tangent ad_a_b = ad_a * b;
224  Tangent lie_bracket_a_b = Group::vee(
225  Group::hat(a) * Group::hat(b) - Group::hat(b) * Group::hat(a));
226  SOPHUS_ASSERT_NEAR(
227  ad_a_b,
228  lie_bracket_a_b,
229  10 * kEpsilonSqrt<Scalar>,
230  "`Ad_A vee(B) == vee([A, B])` Test for {}\n"
231  "Ad_a: {}\n"
232  "a # {} ({})\n"
233  "b # {} ({})",
234  group_name,
235  ad_a.transpose(),
236  tangent_a_id,
237  a.transpose(),
238  tangent_b_id,
239  b.transpose());
240 
241  if constexpr (kDof > 0) {
242  Eigen::Matrix<Scalar, kDof, kDof> const num_diff_ad_a =
243  vectorFieldNumDiff<Scalar, kDof, kDof>(
244  [a](Tangent const& x) {
245  return Group::vee(
246  Group::hat(a) * Group::hat(x) -
247  Group::hat(x) * Group::hat(a));
248  },
249  b);
250 
251  SOPHUS_ASSERT_NEAR(
252  ad_a,
253  num_diff_ad_a,
254  10 * kEpsilonSqrt<Scalar>,
255  "`Ad_A == d/dx [a, x]` Test for {}\n");
256  }
257  }
258  }
259  }
260 
261  static auto hatTests(std::string group_name) -> void {
262  for (size_t i = 0; i < kTangentExamples.size(); ++i) {
263  Tangent tangent = SOPHUS_AT(kTangentExamples, i);
264  SOPHUS_ASSERT_NEAR(
265  tangent,
266  Group::vee(Group::hat(tangent)),
267  kEpsilonSqrt<Scalar>,
268  "`t = vee(hat(t))` Test for {}, tangent #{}: {}",
269  group_name,
270  i,
271  tangent.transpose());
272  }
273  }
274 
275  static auto groupOperationTests(std::string group_name) -> void {
276  for (size_t g_id1 = 0; g_id1 < kElementExamples.size(); ++g_id1) {
277  Group g1 = SOPHUS_AT(kElementExamples, g_id1);
278  for (size_t g_id2 = 0; g_id2 < kElementExamples.size(); ++g_id2) {
279  Group g2 = SOPHUS_AT(kElementExamples, g_id2);
280  for (size_t g_id3 = 0; g_id3 < kElementExamples.size(); ++g_id3) {
281  Group g3 = SOPHUS_AT(kElementExamples, g_id3);
282 
283  Group left_hugging = (g1 * g2) * g3;
284  Group right_hugging = g1 * (g2 * g3);
285  SOPHUS_ASSERT_NEAR(
286  left_hugging.compactMatrix(),
287  right_hugging.compactMatrix(),
288  10.0 * kEpsilonSqrt<Scalar>,
289  "`(g1*g2)*g3 == g1*(g2*g3)` Test for {}, #{}/#{}/#{}",
290  group_name,
291  g_id1,
292  g_id2,
293  g_id3);
294  }
295  }
296  }
297 
298  for (size_t g_id1 = 0; g_id1 < kElementExamples.size(); ++g_id1) {
299  Group foo_from_bar_transform = SOPHUS_AT(kElementExamples, g_id1);
300  for (size_t g_id2 = 0; g_id2 < kElementExamples.size(); ++g_id2) {
301  Group bar_from_daz_transform = SOPHUS_AT(kElementExamples, g_id2);
302 
303  Group daz_from_foo_transform_1 =
304  bar_from_daz_transform.inverse() * foo_from_bar_transform.inverse();
305  Group daz_from_foo_transform_2 =
306  (foo_from_bar_transform * bar_from_daz_transform).inverse();
307 
308  SOPHUS_ASSERT_NEAR(
309  daz_from_foo_transform_1.compactMatrix(),
310  daz_from_foo_transform_2.compactMatrix(),
311  10 * kEpsilonSqrt<Scalar>,
312  "`ing(g2) * inv(g1) == inv(g1 *g2)` Test for {}, #{}/#{}",
313  group_name,
314  g_id1,
315  g_id2);
316  }
317  }
318  }
319 
320  static auto groupActionTests(std::string group_name) -> void {
321  for (size_t point_id = 0; point_id < kPointExamples.size(); ++point_id) {
322  auto point_in = SOPHUS_AT(kPointExamples, point_id);
323  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
324  Group g = SOPHUS_AT(kElementExamples, g_id);
325  Point out_point_from_matrix =
326  g.compactMatrix() * Group::toAmbient(point_in);
327  Point out_point_from_action = g * point_in;
328 
329  SOPHUS_ASSERT_NEAR(
330  out_point_from_matrix,
331  out_point_from_action,
332  kEpsilonSqrt<Scalar>,
333  "`g.matrix() * point == g * point`: {}\n"
334  "point # {} ({})\n"
335  "params # {} ({}); matrix:\n"
336  "{}",
337  group_name,
338  point_id,
339  point_in.transpose(),
340  g_id,
341  g.params().transpose(),
342  g.compactMatrix());
343 
344  Point in_point_through_inverse = g.inverse() * out_point_from_matrix;
345  SOPHUS_ASSERT_NEAR(
346  point_in,
347  in_point_through_inverse,
348  10.0 * kEpsilonSqrt<Scalar>,
349  "`inv(g) * (g * point) == point` Test for {}\n"
350  "point # {} ({})\n"
351  "params # {} ({}); matrix:\n"
352  "{}",
353  group_name,
354  point_id,
355  point_in.transpose(),
356  g_id,
357  g.params().transpose(),
358  g.compactMatrix());
359  }
360  }
361  }
362 
363  static void expJacobiansTest(std::string group_name) {
364  if constexpr (kDof == 0) {
365  return;
366  } else {
367  // for (size_t j = 0; j < kTangentExamples.size(); ++j) {
368  // Tangent a = SOPHUS_AT(kTangentExamples, j);
369  // Eigen::Matrix<Scalar, kNumParams, kDof> d = Group::dxExpX(a);
370  // Eigen::Matrix<Scalar, kNumParams, kDof> j_num =
371  // vectorFieldNumDiff<Scalar, kNumParams, kDof>(
372  // [](Tangent const& x) -> Params {
373  // return Group::exp(x).params();
374  // },
375  // a);
376 
377  // SOPHUS_ASSERT_NEAR(
378  // d,
379  // j_num,
380  // 10.0 * kEpsilonSqrt<Scalar>,
381  // "`dx exp(x)` Test {}",
382  // group_name);
383  // }
384 
385  Tangent o;
386  o.setZero();
387  Eigen::Matrix<Scalar, kNumParams, kDof> j = Group::dxExpXAt0();
388  Eigen::Matrix<Scalar, kNumParams, kDof> j_num =
389  vectorFieldNumDiff<Scalar, kNumParams, kDof>(
390  [](Tangent const& x) -> Params {
391  Params p = Group::exp(x).params();
392  return p;
393  },
394  o);
395  SOPHUS_ASSERT_NEAR(
396  j,
397  j_num,
398  10.0 * kEpsilonSqrt<Scalar>,
399  "`dx exp(x)|x=0` Test {}",
400  group_name);
401 
402  for (size_t point_id = 0; point_id < kPointExamples.size(); ++point_id) {
403  Point point_in = SOPHUS_AT(kPointExamples, point_id);
404  Eigen::Matrix<Scalar, kPointDim, kDof> j =
405  Group::dxExpXTimesPointAt0(point_in);
406  Tangent o;
407  o.setZero();
408  Eigen::Matrix<Scalar, kPointDim, kDof> const j_num =
409  vectorFieldNumDiff<Scalar, kPointDim, kDof>(
410  [point_in](Tangent const& x) {
411  return Group::exp(x) * point_in;
412  },
413  o);
414 
415  SOPHUS_ASSERT_NEAR(
416  j,
417  j_num,
418  10.0 * kEpsilonSqrt<Scalar>,
419  "expJacobiansTest #1: {}",
420  group_name);
421  }
422 
423  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
424  Group g = SOPHUS_AT(kElementExamples, g_id);
425 
426  Tangent o;
427  o.setZero();
428  Eigen::Matrix<Scalar, kNumParams, kDof> j = g.dxThisMulExpXAt0();
429  Eigen::Matrix<Scalar, kNumParams, kDof> j_num =
430  vectorFieldNumDiff<Scalar, kNumParams, kDof>(
431  [g](Tangent const& x) -> Params {
432  return (g * Group::exp(x)).params();
433  },
434  o);
435 
436  SOPHUS_ASSERT_NEAR(
437  j,
438  j_num,
439  10.0 * kEpsilonSqrt<Scalar>,
440  "expJacobiansTest #2: {}",
441  group_name);
442  }
443 
444  for (size_t g_id = 0; g_id < kElementExamples.size(); ++g_id) {
445  Group g = SOPHUS_AT(kElementExamples, g_id);
446 
447  Eigen::Matrix<Scalar, kDof, kDof> j =
448  g.dxLogThisInvTimesXAtThis() * g.dxThisMulExpXAt0();
449  Eigen::Matrix<Scalar, kDof, kDof> j_exp =
450  Eigen::Matrix<Scalar, kDof, kDof>::Identity();
451 
452  SOPHUS_ASSERT_NEAR(
453  j,
454  j_exp,
455  10.0 * kEpsilonSqrt<Scalar>,
456  "expJacobiansTest #3: {}",
457  group_name);
458  }
459  }
460  }
461 
462  static auto runAllTests(std::string group_name) -> void {
463  preservabilityTests(group_name);
464  expTests(group_name);
465  adjointTests(group_name);
466  hatTests(group_name);
467 
468  groupOperationTests(group_name);
469  groupActionTests(group_name);
470 
471  expJacobiansTest(group_name);
472  }
473 };
474 
475 template <concepts::LieGroup TGroup>
476 decltype(TGroup::elementExamples())
477  const LieGroupPropTestSuite<TGroup>::kElementExamples =
478  TGroup::elementExamples();
479 
480 template <concepts::LieGroup TGroup>
481 decltype(TGroup::tangentExamples())
482  const LieGroupPropTestSuite<TGroup>::kTangentExamples =
483  TGroup::tangentExamples();
484 
485 template <concepts::LieGroup TGroup>
486 decltype(pointExamples<typename TGroup::Scalar, TGroup::kPointDim>())
487  const LieGroupPropTestSuite<TGroup>::kPointExamples =
488  pointExamples<Scalar, kPointDim>();
489 
490 // using namespace sophus;
491 
492 // // bool contructorAndAssignmentTest() {
493 // // bool passed = true;
494 // // for (Group foo_transform_bar : group_vec_) {
495 // // Group foo_t2_bar = foo_transform_bar;
496 // // SOPHUS_TEST_APPROX(
497 // // passed,
498 // // foo_transform_bar.matrix(),
499 // // foo_t2_bar.matrix(),
500 // // small_eps,
501 // // "Copy constructor: %\nvs\n %",
502 // // transpose(foo_transform_bar.matrix()),
503 // // transpose(foo_t2_bar.matrix()));
504 // // Group foo_t3_bar;
505 // // foo_t3_bar = foo_transform_bar;
506 // // SOPHUS_TEST_APPROX(
507 // // passed,
508 // // foo_transform_bar.matrix(),
509 // // foo_t3_bar.matrix(),
510 // // small_eps,
511 // // "Copy assignment: %\nvs\n %",
512 // // transpose(foo_transform_bar.matrix()),
513 // // transpose(foo_t3_bar.matrix()));
514 
515 // // Group foo_t4_bar(foo_transform_bar.matrix());
516 // // SOPHUS_TEST_APPROX(
517 // // passed,
518 // // foo_transform_bar.matrix(),
519 // // foo_t4_bar.matrix(),
520 // // small_eps,
521 // // "Constructor from homogeneous matrix: %\nvs\n %",
522 // // transpose(foo_transform_bar.matrix()),
523 // // transpose(foo_t4_bar.matrix()));
524 
525 // // Eigen::Map<Group> foo_tmap_bar(foo_transform_bar.data());
526 // // Group foo_t5_bar = foo_tmap_bar;
527 // // SOPHUS_TEST_APPROX(
528 // // passed,
529 // // foo_transform_bar.matrix(),
530 // // foo_t5_bar.matrix(),
531 // // small_eps,
532 // // "Assignment from Eigen::Map type: %\nvs\n %",
533 // // transpose(foo_transform_bar.matrix()),
534 // // transpose(foo_t5_bar.matrix()));
535 
536 // // Eigen::Map<Group const> foo_tcmap_bar(foo_transform_bar.data());
537 // // Group foo_t6_bar;
538 // // foo_t6_bar = foo_tcmap_bar;
539 // // SOPHUS_TEST_APPROX(
540 // // passed,
541 // // foo_transform_bar.matrix(),
542 // // foo_t5_bar.matrix(),
543 // // small_eps,
544 // // "Assignment from Eigen::Map type: %\nvs\n %",
545 // // transpose(foo_transform_bar.matrix()),
546 // // transpose(foo_t5_bar.matrix()));
547 
548 // // Group i;
549 // // Eigen::Map<Group> foo_tmap2_bar(i.data());
550 // // foo_tmap2_bar = foo_transform_bar;
551 // // SOPHUS_TEST_APPROX(
552 // // passed,
553 // // foo_tmap2_bar.matrix(),
554 // // foo_transform_bar.matrix(),
555 // // small_eps,
556 // // "Assignment to Eigen::Map type: %\nvs\n %",
557 // // transpose(foo_tmap2_bar.matrix()),
558 // // transpose(foo_transform_bar.matrix()));
559 // // }
560 // // return passed;
561 // // }
562 
563 // // bool derivativeTest() {
564 // // bool passed = true;
565 
566 // // Group g;
567 // // for (int i = 0; i < kDof; ++i) {
568 // // Transformation gi = g.dxiExpmatXAt0(i);
569 // // Transformation gi2 = curveNumDiff(
570 // // [i](Scalar xi) -> Transformation {
571 // // Tangent x;
572 // // setToZero(x);
573 // // setElementAt(x, xi, i);
574 // // return Group::exp(x).matrix();
575 // // },
576 // // Scalar(0));
577 // // SOPHUS_TEST_APPROX(
578 // // passed, gi, gi2, small_eps_sqrt, "Dxi_exp_x_matrix_at_ case
579 // %", i);
580 // // }
581 
582 // // return passed;
583 // // }
584 
585 // // template <class G>
586 // // void productTest(std::string group_name) {
587 // // bool passed = true;
588 
589 // // for (size_t params_id = 1; params_id < G::paramsExamples().size();
590 // // ++params_id) {
591 // // auto params1 = G::paramsExamples()[params_id - 1];
592 // // auto params2 = G::paramsExamples()[params_id];
593 // // auto g1 = G::fromParams(params);
594 // // auto g2 = G::fromParams(params);
595 
596 // // G product = g1 * g2;
597 // // g1 *= g2;
598 // // SOPHUS_ASSERT_NEAR(
599 // // g1.matrix(), product.matrix(), small_eps, "Product case: %", i);
600 // // }
601 // // return passed;
602 // // }
603 
604 // // bool expMapTest() {
605 // // bool passed = true;
606 // // for (size_t i = 0; i < tangent_vec_.size(); ++i) {
607 // // Tangent omega = tangent_vec_[i];
608 // // Transformation exp_x = Group::exp(omega).matrix();
609 // // Transformation expmap_hat_x = (Group::hat(omega)).exp();
610 // // SOPHUS_TEST_APPROX(
611 // // passed,
612 // // exp_x,
613 // // expmap_hat_x,
614 // // Scalar(10) * small_eps,
615 // // "expmap(hat(x)) - exp(x) case: %",
616 // // i);
617 // // }
618 // // return passed;
619 // // }
620 
621 // // template <class G>
622 // // bool lieBracketTest(std::string group_name) {
623 // // for (size_t i = 0; i < tangent_vec_.size(); ++i) {
624 // // for (size_t j = 0; j < tangent_vec_.size(); ++j) {
625 // // Tangent tangent1 = Group::lieBracket(tangent_vec_[i],
626 // // tangent_vec_[j]); Transformation hati =
627 // Group::hat(tangent_vec_[i]);
628 // // Transformation hatj = Group::hat(tangent_vec_[j]);
629 
630 // // Tangent tangent2 = Group::vee(hati * hatj - hatj * hati);
631 // // SOPHUS_ASSERT_NEAR(
632 // // passed,
633 // // tangent1,
634 // // tangent2,
635 // // small_eps,
636 // // "lieBracketTest {}",
637 // // group_name);
638 // // }
639 // // }
640 // // }
641 
642 // // template <class TS = Scalar>
643 // // std::enable_if_t<std::is_same_v<TS, float>::value, bool> testSpline()
644 // {
645 // // // skip tests for Scalar == float
646 // // return true;
647 // // }
648 } // namespace test
649 } // namespace sophus
lie_group.h
sophus::test::LieGroupPropTestSuite::CompactMatrix
Eigen::Matrix< Scalar, kPointDim, kAmbientDim > CompactMatrix
Definition: lie_group_prop_tests.h:40
Eigen
Definition: params.h:72
sophus::test::LieGroupPropTestSuite::Group
TGroup Group
Definition: lie_group_prop_tests.h:22
sophus::UnitVector::fromVectorAndNormalize
static auto fromVectorAndNormalize(Eigen::Matrix< TScalar, kN, 1 > const &v) -> UnitVector
Definition: unit_vector.h:180
num_diff.h
sophus::UnitVector
Definition: lie_group.h:14
sophus::test::LieGroupPropTestSuite::Scalar
typename Group::Scalar Scalar
Definition: lie_group_prop_tests.h:24
sophus::test::LieGroupPropTestSuite::preservabilityTests
static auto preservabilityTests(std::string group_name) -> void
Definition: lie_group_prop_tests.h:42
sophus
Image MutImage, owning images types.
Definition: num_diff.h:20
sophus::test::LieGroupPropTestSuite::kPointExamples
static decltype(pointExamples< Scalar, Group::kPointDim >()) const kPointExamples
Definition: lie_group_prop_tests.h:34
sophus::test::LieGroupPropTestSuite::kDof
static constexpr int kDof
Definition: lie_group_prop_tests.h:26
sophus::test::LieGroupPropTestSuite::expJacobiansTest
static void expJacobiansTest(std::string group_name)
Definition: lie_group_prop_tests.h:363
sophus::test::LieGroupPropTestSuite::hatTests
static auto hatTests(std::string group_name) -> void
Definition: lie_group_prop_tests.h:261
sophus::pointExamples
auto pointExamples()
Definition: vector_space.h:307
sophus::test::LieGroupPropTestSuite::Tangent
Eigen::Vector< Scalar, kDof > Tangent
Definition: lie_group_prop_tests.h:36
sophus::test::LieGroupPropTestSuite::Params
Eigen::Vector< Scalar, kNumParams > Params
Definition: lie_group_prop_tests.h:37
sophus::test::LieGroupPropTestSuite::Matrix
Eigen::Matrix< Scalar, kAmbientDim, kAmbientDim > Matrix
Definition: lie_group_prop_tests.h:39
sophus::test::LieGroupPropTestSuite::kElementExamples
static decltype(Group::elementExamples()) const kElementExamples
Definition: lie_group_prop_tests.h:31
sophus::test::LieGroupPropTestSuite::kNumParams
static constexpr int kNumParams
Definition: lie_group_prop_tests.h:27
SOPHUS_ASSERT_NEAR
#define SOPHUS_ASSERT_NEAR(...)
Definition: common.h:47
sophus::test::LieGroupPropTestSuite::groupOperationTests
static auto groupOperationTests(std::string group_name) -> void
Definition: lie_group_prop_tests.h:275
FARM_ASSERT_LE
#define FARM_ASSERT_LE(lhs, rhs,...)
If it is false that lhs <= rhs, print formatted error message and then panic.
Definition: logger.h:289
sophus::test::LieGroupPropTestSuite::kPointDim
static constexpr int kPointDim
Definition: lie_group_prop_tests.h:28
sophus::test::LieGroupPropTestSuite::groupActionTests
static auto groupActionTests(std::string group_name) -> void
Definition: lie_group_prop_tests.h:320
sophus::concepts::LieGroup
concept LieGroup
Definition: lie_group.h:170
sophus::test::LieGroupPropTestSuite::adjointTests
static auto adjointTests(std::string group_name) -> void
Definition: lie_group_prop_tests.h:184
sophus::UnitVector::fromUnitVector
static auto fromUnitVector(Eigen::Matrix< TScalar, kN, 1 > const &v) -> UnitVector
Definition: unit_vector.h:175
sophus::test::LieGroupPropTestSuite::kAmbientDim
static constexpr int kAmbientDim
Definition: lie_group_prop_tests.h:29
sophus::test::LieGroupPropTestSuite::expTests
static auto expTests(std::string group_name) -> void
Definition: lie_group_prop_tests.h:142
SOPHUS_AT
#define SOPHUS_AT(...)
Definition: common.h:48
sophus::test::LieGroupPropTestSuite::runAllTests
static auto runAllTests(std::string group_name) -> void
Definition: lie_group_prop_tests.h:462
vector_space.h
sophus::test::LieGroupPropTestSuite
Definition: lie_group_prop_tests.h:21
sophus::test::LieGroupPropTestSuite::Point
Eigen::Vector< Scalar, kPointDim > Point
Definition: lie_group_prop_tests.h:38
sophus::test::LieGroupPropTestSuite::kTangentExamples
static decltype(Group::tangentExamples()) const kTangentExamples
Definition: lie_group_prop_tests.h:32