tests.hpp
Go to the documentation of this file.
1 #ifndef SOPUHS_TESTS_HPP
2 #define SOPUHS_TESTS_HPP
3 
4 #include <array>
5 
6 #include <Eigen/StdVector>
7 #include <unsupported/Eigen/MatrixFunctions>
8 
9 #include <sophus/average.hpp>
10 #include <sophus/interpolate.hpp>
11 #include <sophus/num_diff.hpp>
12 #include <sophus/test_macros.hpp>
13 
14 #ifdef SOPHUS_CERES
15 #include <ceres/jet.h>
16 #endif
17 
18 namespace Sophus {
19 
20 template <class LieGroup_>
21 class LieGroupTests {
22  public:
23  using LieGroup = LieGroup_;
24  using Scalar = typename LieGroup::Scalar;
25  using Transformation = typename LieGroup::Transformation;
26  using Tangent = typename LieGroup::Tangent;
27  using Point = typename LieGroup::Point;
28  using HomogeneousPoint = typename LieGroup::HomogeneousPoint;
29  using ConstPointMap = Eigen::Map<const Point>;
30  using Line = typename LieGroup::Line;
31  using Adjoint = typename LieGroup::Adjoint;
32  static int constexpr N = LieGroup::N;
33  static int constexpr DoF = LieGroup::DoF;
34  static int constexpr num_parameters = LieGroup::num_parameters;
35 
36  LieGroupTests(
37  std::vector<LieGroup, Eigen::aligned_allocator<LieGroup>> const&
38  group_vec,
39  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> const&
40  tangent_vec,
41  std::vector<Point, Eigen::aligned_allocator<Point>> const& point_vec)
42  : group_vec_(group_vec),
43  tangent_vec_(tangent_vec),
44  point_vec_(point_vec) {}
45 
46  bool adjointTest() {
47  bool passed = true;
48  for (size_t i = 0; i < group_vec_.size(); ++i) {
49  Transformation T = group_vec_[i].matrix();
50  Adjoint Ad = group_vec_[i].Adj();
51  for (size_t j = 0; j < tangent_vec_.size(); ++j) {
52  Tangent x = tangent_vec_[j];
53 
54  Transformation I;
55  I.setIdentity();
56  Tangent ad1 = Ad * x;
57  Tangent ad2 = LieGroup::vee(T * LieGroup::hat(x) *
58  group_vec_[i].inverse().matrix());
59  SOPHUS_TEST_APPROX(passed, ad1, ad2, Scalar(10) * kSmallEps,
60  "Adjoint case %, %", i, j);
61  }
62  }
63  return passed;
64  }
65 
66  bool contructorAndAssignmentTest() {
67  bool passed = true;
68  for (LieGroup foo_T_bar : group_vec_) {
69  LieGroup foo_T2_bar = foo_T_bar;
70  SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T2_bar.matrix(),
71  kSmallEps, "Copy constructor: %\nvs\n %",
72  transpose(foo_T_bar.matrix()),
73  transpose(foo_T2_bar.matrix()));
74  LieGroup foo_T3_bar;
75  foo_T3_bar = foo_T_bar;
76  SOPHUS_TEST_APPROX(passed, foo_T_bar.matrix(), foo_T3_bar.matrix(),
77  kSmallEps, "Copy assignment: %\nvs\n %",
78  transpose(foo_T_bar.matrix()),
79  transpose(foo_T3_bar.matrix()));
80 
81  LieGroup foo_T4_bar(foo_T_bar.matrix());
82  SOPHUS_TEST_APPROX(
83  passed, foo_T_bar.matrix(), foo_T4_bar.matrix(), kSmallEps,
84  "Constructor from homogeneous matrix: %\nvs\n %",
85  transpose(foo_T_bar.matrix()), transpose(foo_T4_bar.matrix()));
86 
87  Eigen::Map<LieGroup> foo_Tmap_bar(foo_T_bar.data());
88  LieGroup foo_T5_bar = foo_Tmap_bar;
89  SOPHUS_TEST_APPROX(
90  passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), kSmallEps,
91  "Assignment from Eigen::Map type: %\nvs\n %",
92  transpose(foo_T_bar.matrix()), transpose(foo_T5_bar.matrix()));
93 
94  Eigen::Map<LieGroup const> foo_Tcmap_bar(foo_T_bar.data());
95  LieGroup foo_T6_bar;
96  foo_T6_bar = foo_Tcmap_bar;
97  SOPHUS_TEST_APPROX(
98  passed, foo_T_bar.matrix(), foo_T5_bar.matrix(), kSmallEps,
99  "Assignment from Eigen::Map type: %\nvs\n %",
100  transpose(foo_T_bar.matrix()), transpose(foo_T5_bar.matrix()));
101 
102  LieGroup I;
103  Eigen::Map<LieGroup> foo_Tmap2_bar(I.data());
104  foo_Tmap2_bar = foo_T_bar;
105  SOPHUS_TEST_APPROX(passed, foo_Tmap2_bar.matrix(), foo_T_bar.matrix(),
106  kSmallEps, "Assignment to Eigen::Map type: %\nvs\n %",
107  transpose(foo_Tmap2_bar.matrix()),
108  transpose(foo_T_bar.matrix()));
109  }
110  return passed;
111  }
112 
113  bool derivativeTest() {
114  bool passed = true;
115 
116  LieGroup g;
117  for (int i = 0; i < DoF; ++i) {
118  Transformation Gi = g.Dxi_exp_x_matrix_at_0(i);
119  Transformation Gi2 = curveNumDiff(
120  [i](Scalar xi) -> Transformation {
121  Tangent x;
122  setToZero(x);
123  setElementAt(x, xi, i);
124  return LieGroup::exp(x).matrix();
125  },
126  Scalar(0));
127  SOPHUS_TEST_APPROX(passed, Gi, Gi2, kSmallEpsSqrt,
128  "Dxi_exp_x_matrix_at_ case %", i);
129  }
130 
131  return passed;
132  }
133 
134  template <class G = LieGroup>
135  enable_if_t<std::is_same<G, Sophus::SO2<Scalar>>::value ||
136  std::is_same<G, Sophus::SO3<Scalar>>::value ||
137  std::is_same<G, Sophus::SE2<Scalar>>::value ||
138  std::is_same<G, Sophus::SE3<Scalar>>::value,
139  bool>
140  additionalDerivativeTest() {
141  bool passed = true;
142  for (size_t j = 0; j < tangent_vec_.size(); ++j) {
143  Tangent a = tangent_vec_[j];
144  Eigen::Matrix<Scalar, num_parameters, DoF> J = LieGroup::Dx_exp_x(a);
145  Eigen::Matrix<Scalar, num_parameters, DoF> J_num =
146  vectorFieldNumDiff<Scalar, num_parameters, DoF>(
147  [](Tangent const& x) -> Sophus::Vector<Scalar, num_parameters> {
148  return LieGroup::exp(x).params();
149  },
150  a);
151 
152  SOPHUS_TEST_APPROX(passed, J, J_num, 3 * kSmallEpsSqrt,
153  "Dx_exp_x case: %", j);
154  }
155 
156  Tangent o;
157  setToZero(o);
158  Eigen::Matrix<Scalar, num_parameters, DoF> J = LieGroup::Dx_exp_x_at_0();
159  Eigen::Matrix<Scalar, num_parameters, DoF> J_num =
160  vectorFieldNumDiff<Scalar, num_parameters, DoF>(
161  [](Tangent const& x) -> Sophus::Vector<Scalar, num_parameters> {
162  return LieGroup::exp(x).params();
163  },
164  o);
165  SOPHUS_TEST_APPROX(passed, J, J_num, kSmallEpsSqrt, "Dx_exp_x_at_0");
166 
167  for (size_t i = 0; i < group_vec_.size(); ++i) {
168  LieGroup T = group_vec_[i];
169 
170  Eigen::Matrix<Scalar, num_parameters, DoF> J = T.Dx_this_mul_exp_x_at_0();
171  Eigen::Matrix<Scalar, num_parameters, DoF> J_num =
172  vectorFieldNumDiff<Scalar, num_parameters, DoF>(
173  [T](Tangent const& x) -> Sophus::Vector<Scalar, num_parameters> {
174  return (T * LieGroup::exp(x)).params();
175  },
176  o);
177 
178  SOPHUS_TEST_APPROX(passed, J, J_num, kSmallEpsSqrt,
179  "Dx_this_mul_exp_x_at_0 case: %", i);
180  }
181 
182  return passed;
183  }
184 
185  template <class G = LieGroup>
186  enable_if_t<!std::is_same<G, Sophus::SO2<Scalar>>::value &&
187  !std::is_same<G, Sophus::SO3<Scalar>>::value &&
188  !std::is_same<G, Sophus::SE2<Scalar>>::value &&
189  !std::is_same<G, Sophus::SE3<Scalar>>::value,
190  bool>
191  additionalDerivativeTest() {
192  return true;
193  }
194 
195  bool productTest() {
196  bool passed = true;
197 
198  for (size_t i = 0; i < group_vec_.size() - 1; ++i) {
199  LieGroup T1 = group_vec_[i];
200  LieGroup T2 = group_vec_[i + 1];
201  LieGroup mult = T1 * T2;
202  T1 *= T2;
203  SOPHUS_TEST_APPROX(passed, T1.matrix(), mult.matrix(), kSmallEps,
204  "Product case: %", i);
205  }
206  return passed;
207  }
208 
209  bool expLogTest() {
210  bool passed = true;
211 
212  for (size_t i = 0; i < group_vec_.size(); ++i) {
213  Transformation T1 = group_vec_[i].matrix();
214  Transformation T2 = LieGroup::exp(group_vec_[i].log()).matrix();
215  SOPHUS_TEST_APPROX(passed, T1, T2, kSmallEps, "G - exp(log(G)) case: %",
216  i);
217  }
218  return passed;
219  }
220 
221  bool expMapTest() {
222  bool passed = true;
223  for (size_t i = 0; i < tangent_vec_.size(); ++i) {
224  Tangent omega = tangent_vec_[i];
225  Transformation exp_x = LieGroup::exp(omega).matrix();
226  Transformation expmap_hat_x = (LieGroup::hat(omega)).exp();
227  SOPHUS_TEST_APPROX(passed, exp_x, expmap_hat_x, Scalar(10) * kSmallEps,
228  "expmap(hat(x)) - exp(x) case: %", i);
229  }
230  return passed;
231  }
232 
233  bool groupActionTest() {
234  bool passed = true;
235 
236  for (size_t i = 0; i < group_vec_.size(); ++i) {
237  for (size_t j = 0; j < point_vec_.size(); ++j) {
238  Point const& p = point_vec_[j];
239  Point point1 = group_vec_[i] * p;
240 
241  HomogeneousPoint hp = p.homogeneous();
242  HomogeneousPoint hpoint1 = group_vec_[i] * hp;
243 
244  ConstPointMap p_map(p.data());
245  Point pointmap1 = group_vec_[i] * p_map;
246 
247  Transformation T = group_vec_[i].matrix();
248  Point gt_point1 = map(T, p);
249 
250  SOPHUS_TEST_APPROX(passed, point1, gt_point1, kSmallEps,
251  "Transform point case: %", i);
252  SOPHUS_TEST_APPROX(passed, hpoint1.hnormalized().eval(), gt_point1,
253  kSmallEps, "Transform homogeneous point case: %", i);
254  SOPHUS_TEST_APPROX(passed, pointmap1, gt_point1, kSmallEps,
255  "Transform map point case: %", i);
256  }
257  }
258  return passed;
259  }
260 
261  bool lineActionTest() {
262  bool passed = point_vec_.size() > 1;
263 
264  for (size_t i = 0; i < group_vec_.size(); ++i) {
265  for (size_t j = 0; j + 1 < point_vec_.size(); ++j) {
266  Point const& p1 = point_vec_[j];
267  Point const& p2 = point_vec_[j + 1];
268  Line l = Line::Through(p1, p2);
269  Point p1_t = group_vec_[i] * p1;
270  Point p2_t = group_vec_[i] * p2;
271  Line l_t = group_vec_[i] * l;
272 
273  SOPHUS_TEST_APPROX(passed, l_t.squaredDistance(p1_t),
274  static_cast<Scalar>(0), kSmallEps,
275  "Transform line case (1st point) : %", i);
276  SOPHUS_TEST_APPROX(passed, l_t.squaredDistance(p2_t),
277  static_cast<Scalar>(0), kSmallEps,
278  "Transform line case (2nd point) : %", i);
279  SOPHUS_TEST_APPROX(passed, l_t.direction().squaredNorm(),
280  l.direction().squaredNorm(), kSmallEps,
281  "Transform line case (direction) : %", i);
282  }
283  }
284  return passed;
285  }
286 
287  bool lieBracketTest() {
288  bool passed = true;
289  for (size_t i = 0; i < tangent_vec_.size(); ++i) {
290  for (size_t j = 0; j < tangent_vec_.size(); ++j) {
291  Tangent tangent1 =
292  LieGroup::lieBracket(tangent_vec_[i], tangent_vec_[j]);
293  Transformation hati = LieGroup::hat(tangent_vec_[i]);
294  Transformation hatj = LieGroup::hat(tangent_vec_[j]);
295 
296  Tangent tangent2 = LieGroup::vee(hati * hatj - hatj * hati);
297  SOPHUS_TEST_APPROX(passed, tangent1, tangent2, kSmallEps,
298  "Lie Bracket case: %", i);
299  }
300  }
301  return passed;
302  }
303 
304  bool veeHatTest() {
305  bool passed = true;
306  for (size_t i = 0; i < tangent_vec_.size(); ++i) {
307  SOPHUS_TEST_APPROX(passed, Tangent(tangent_vec_[i]),
308  LieGroup::vee(LieGroup::hat(tangent_vec_[i])),
309  kSmallEps, "Hat-vee case: %", i);
310  }
311  return passed;
312  }
313 
314  bool newDeleteSmokeTest() {
315  bool passed = true;
316  LieGroup* raw_ptr = nullptr;
317  raw_ptr = new LieGroup();
318  SOPHUS_TEST_NEQ(passed, reinterpret_cast<std::uintptr_t>(raw_ptr), 0);
319  delete raw_ptr;
320  return passed;
321  }
322 
323  bool interpolateAndMeanTest() {
324  bool passed = true;
325  using std::sqrt;
326  Scalar const eps = Constants<Scalar>::epsilon();
327  Scalar const sqrt_eps = sqrt(eps);
328  // TODO: Improve accuracy of ``interpolate`` (and hence ``exp`` and ``log``)
329  // so that we can use more accurate bounds in these tests, i.e.
330  // ``eps`` instead of ``sqrt_eps``.
331 
332  for (LieGroup const& foo_T_bar : group_vec_) {
333  for (LieGroup const& foo_T_baz : group_vec_) {
334  // Test boundary conditions ``alpha=0`` and ``alpha=1``.
335  LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, Scalar(0));
336  SOPHUS_TEST_APPROX(passed, foo_T_quiz.matrix(), foo_T_bar.matrix(),
337  sqrt_eps);
338  foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, Scalar(1));
339  SOPHUS_TEST_APPROX(passed, foo_T_quiz.matrix(), foo_T_baz.matrix(),
340  sqrt_eps);
341  }
342  }
343  for (Scalar alpha :
344  {Scalar(0.1), Scalar(0.5), Scalar(0.75), Scalar(0.99)}) {
345  for (LieGroup const& foo_T_bar : group_vec_) {
346  for (LieGroup const& foo_T_baz : group_vec_) {
347  LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, alpha);
348  // test left-invariance:
349  //
350  // dash_T_foo * interp(foo_T_bar, foo_T_baz)
351  // == interp(dash_T_foo * foo_T_bar, dash_T_foo * foo_T_baz)
352 
353  if (interp_details::Traits<LieGroup>::hasShortestPathAmbiguity(
354  foo_T_bar.inverse() * foo_T_baz)) {
355  // skip check since there is a shortest path ambiguity
356  continue;
357  }
358  for (LieGroup const& dash_T_foo : group_vec_) {
359  LieGroup dash_T_quiz = interpolate(dash_T_foo * foo_T_bar,
360  dash_T_foo * foo_T_baz, alpha);
361  SOPHUS_TEST_APPROX(passed, dash_T_quiz.matrix(),
362  (dash_T_foo * foo_T_quiz).matrix(), sqrt_eps);
363  }
364  // test inverse-invariance:
365  //
366  // interp(foo_T_bar, foo_T_baz).inverse()
367  // == interp(foo_T_bar.inverse(), dash_T_foo.inverse())
368  LieGroup quiz_T_foo =
369  interpolate(foo_T_bar.inverse(), foo_T_baz.inverse(), alpha);
370  SOPHUS_TEST_APPROX(passed, quiz_T_foo.inverse().matrix(),
371  foo_T_quiz.matrix(), sqrt_eps);
372  }
373  }
374 
375  for (LieGroup const& bar_T_foo : group_vec_) {
376  for (LieGroup const& baz_T_foo : group_vec_) {
377  LieGroup quiz_T_foo = interpolate(bar_T_foo, baz_T_foo, alpha);
378  // test right-invariance:
379  //
380  // interp(bar_T_foo, bar_T_foo) * foo_T_dash
381  // == interp(bar_T_foo * foo_T_dash, bar_T_foo * foo_T_dash)
382 
383  if (interp_details::Traits<LieGroup>::hasShortestPathAmbiguity(
384  bar_T_foo * baz_T_foo.inverse())) {
385  // skip check since there is a shortest path ambiguity
386  continue;
387  }
388  for (LieGroup const& foo_T_dash : group_vec_) {
389  LieGroup quiz_T_dash = interpolate(bar_T_foo * foo_T_dash,
390  baz_T_foo * foo_T_dash, alpha);
391  SOPHUS_TEST_APPROX(passed, quiz_T_dash.matrix(),
392  (quiz_T_foo * foo_T_dash).matrix(), sqrt_eps);
393  }
394  }
395  }
396  }
397 
398  for (LieGroup const& foo_T_bar : group_vec_) {
399  for (LieGroup const& foo_T_baz : group_vec_) {
400  if (interp_details::Traits<LieGroup>::hasShortestPathAmbiguity(
401  foo_T_bar.inverse() * foo_T_baz)) {
402  // skip check since there is a shortest path ambiguity
403  continue;
404  }
405 
406  // test average({A, B}) == interp(A, B):
407  LieGroup foo_T_quiz = interpolate(foo_T_bar, foo_T_baz, 0.5);
408  optional<LieGroup> foo_T_iaverage = iterativeMean(
409  std::array<LieGroup, 2>({{foo_T_bar, foo_T_baz}}), 20);
410  optional<LieGroup> foo_T_average =
411  average(std::array<LieGroup, 2>({{foo_T_bar, foo_T_baz}}));
412  SOPHUS_TEST(passed, bool(foo_T_average),
413  "log(foo_T_bar): %\nlog(foo_T_baz): %",
414  transpose(foo_T_bar.log()), transpose(foo_T_baz.log()));
415  if (foo_T_average) {
416  SOPHUS_TEST_APPROX(
417  passed, foo_T_quiz.matrix(), foo_T_average->matrix(), sqrt_eps,
418  "log(foo_T_bar): %\nlog(foo_T_baz): %\n"
419  "log(interp): %\nlog(average): %",
420  transpose(foo_T_bar.log()), transpose(foo_T_baz.log()),
421  transpose(foo_T_quiz.log()), transpose(foo_T_average->log()));
422  }
423  SOPHUS_TEST(passed, bool(foo_T_iaverage),
424  "log(foo_T_bar): %\nlog(foo_T_baz): %\n"
425  "log(interp): %\nlog(iaverage): %",
426  transpose(foo_T_bar.log()), transpose(foo_T_baz.log()),
427  transpose(foo_T_quiz.log()),
428  transpose(foo_T_iaverage->log()));
429  if (foo_T_iaverage) {
430  SOPHUS_TEST_APPROX(
431  passed, foo_T_quiz.matrix(), foo_T_iaverage->matrix(), sqrt_eps,
432  "log(foo_T_bar): %\nlog(foo_T_baz): %",
433  transpose(foo_T_bar.log()), transpose(foo_T_baz.log()));
434  }
435  }
436  }
437 
438  return passed;
439  }
440 
441  bool testRandomSmoke() {
442  bool passed = true;
443  std::default_random_engine engine;
444  for (int i = 0; i < 100; ++i) {
445  LieGroup g = LieGroup::sampleUniform(engine);
446  SOPHUS_TEST_EQUAL(passed, g.params(), g.params());
447  }
448  return passed;
449  }
450 
451  template <class S = Scalar>
452  enable_if_t<std::is_floating_point<S>::value, bool> doAllTestsPass() {
453  return doesLargeTestSetPass();
454  }
455 
456  template <class S = Scalar>
457  enable_if_t<!std::is_floating_point<S>::value, bool> doAllTestsPass() {
458  return doesSmallTestSetPass();
459  }
460 
461  private:
462  bool doesSmallTestSetPass() {
463  bool passed = true;
464  passed &= adjointTest();
465  passed &= contructorAndAssignmentTest();
466  passed &= productTest();
467  passed &= expLogTest();
468  passed &= groupActionTest();
469  passed &= lineActionTest();
470  passed &= lieBracketTest();
471  passed &= veeHatTest();
472  passed &= newDeleteSmokeTest();
473  return passed;
474  }
475 
476  bool doesLargeTestSetPass() {
477  bool passed = true;
478  passed &= doesSmallTestSetPass();
479  passed &= additionalDerivativeTest();
480  passed &= derivativeTest();
481  passed &= expMapTest();
482  passed &= interpolateAndMeanTest();
483  passed &= testRandomSmoke();
484  return passed;
485  }
486 
487  Scalar const kSmallEps = Constants<Scalar>::epsilon();
488  Scalar const kSmallEpsSqrt = Constants<Scalar>::epsilonSqrt();
489 
490  Eigen::Matrix<Scalar, N - 1, 1> map(
491  Eigen::Matrix<Scalar, N, N> const& T,
492  Eigen::Matrix<Scalar, N - 1, 1> const& p) {
493  return T.template topLeftCorner<N - 1, N - 1>() * p +
494  T.template topRightCorner<N - 1, 1>();
495  }
496 
497  Eigen::Matrix<Scalar, N, 1> map(Eigen::Matrix<Scalar, N, N> const& T,
498  Eigen::Matrix<Scalar, N, 1> const& p) {
499  return T * p;
500  }
501 
502  std::vector<LieGroup, Eigen::aligned_allocator<LieGroup>> group_vec_;
503  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
504  std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
505 };
506 
507 template <class Scalar>
508 std::vector<SE3<Scalar>, Eigen::aligned_allocator<SE3<Scalar>>> getTestSE3s() {
509  Scalar const kPi = Constants<Scalar>::pi();
510  std::vector<SE3<Scalar>, Eigen::aligned_allocator<SE3<Scalar>>> se3_vec;
511  se3_vec.push_back(SE3<Scalar>(
512  SO3<Scalar>::exp(Vector3<Scalar>(Scalar(0.2), Scalar(0.5), Scalar(0.0))),
513  Vector3<Scalar>(Scalar(0), Scalar(0), Scalar(0))));
514  se3_vec.push_back(SE3<Scalar>(
515  SO3<Scalar>::exp(Vector3<Scalar>(Scalar(0.2), Scalar(0.5), Scalar(-1.0))),
516  Vector3<Scalar>(Scalar(10), Scalar(0), Scalar(0))));
517  se3_vec.push_back(
518  SE3<Scalar>::trans(Vector3<Scalar>(Scalar(0), Scalar(100), Scalar(5))));
519  se3_vec.push_back(SE3<Scalar>::rotZ(Scalar(0.00001)));
520  se3_vec.push_back(
521  SE3<Scalar>::trans(Scalar(0), Scalar(-0.00000001), Scalar(0.0000000001)) *
522  SE3<Scalar>::rotZ(Scalar(0.00001)));
523  se3_vec.push_back(SE3<Scalar>::transX(Scalar(0.01)) *
524  SE3<Scalar>::rotZ(Scalar(0.00001)));
525  se3_vec.push_back(SE3<Scalar>::trans(Scalar(4), Scalar(-5), Scalar(0)) *
526  SE3<Scalar>::rotX(kPi));
527  se3_vec.push_back(
528  SE3<Scalar>(SO3<Scalar>::exp(
529  Vector3<Scalar>(Scalar(0.2), Scalar(0.5), Scalar(0.0))),
530  Vector3<Scalar>(Scalar(0), Scalar(0), Scalar(0))) *
531  SE3<Scalar>::rotX(kPi) *
532  SE3<Scalar>(SO3<Scalar>::exp(Vector3<Scalar>(Scalar(-0.2), Scalar(-0.5),
533  Scalar(-0.0))),
534  Vector3<Scalar>(Scalar(0), Scalar(0), Scalar(0))));
535  se3_vec.push_back(
536  SE3<Scalar>(SO3<Scalar>::exp(
537  Vector3<Scalar>(Scalar(0.3), Scalar(0.5), Scalar(0.1))),
538  Vector3<Scalar>(Scalar(2), Scalar(0), Scalar(-7))) *
539  SE3<Scalar>::rotX(kPi) *
540  SE3<Scalar>(SO3<Scalar>::exp(Vector3<Scalar>(Scalar(-0.3), Scalar(-0.5),
541  Scalar(-0.1))),
542  Vector3<Scalar>(Scalar(0), Scalar(6), Scalar(0))));
543  return se3_vec;
544 }
545 
546 template <class T>
547 std::vector<SE2<T>, Eigen::aligned_allocator<SE2<T>>> getTestSE2s() {
548  T const kPi = Constants<T>::pi();
549  std::vector<SE2<T>, Eigen::aligned_allocator<SE2<T>>> se2_vec;
550  se2_vec.push_back(SE2<T>());
551  se2_vec.push_back(SE2<T>(SO2<T>(0.2), Vector2<T>(10, 0)));
552  se2_vec.push_back(SE2<T>::transY(100));
553  se2_vec.push_back(SE2<T>::trans(Vector2<T>(1, 2)));
554  se2_vec.push_back(SE2<T>(SO2<T>(-1.), Vector2<T>(20, -1)));
555  se2_vec.push_back(
556  SE2<T>(SO2<T>(0.00001), Vector2<T>(-0.00000001, 0.0000000001)));
557  se2_vec.push_back(SE2<T>(SO2<T>(0.3), Vector2<T>(2, 0)) * SE2<T>::rot(kPi) *
558  SE2<T>(SO2<T>(-0.3), Vector2<T>(0, 6)));
559  return se2_vec;
560 }
561 } // namespace Sophus
562 #endif // TESTS_HPP
Sophus::LieGroupTests::expMapTest
bool expMapTest()
Definition: tests.hpp:221
Sophus::LieGroupTests::testRandomSmoke
bool testRandomSmoke()
Definition: tests.hpp:441
Sophus::LieGroupTests::contructorAndAssignmentTest
bool contructorAndAssignmentTest()
Definition: tests.hpp:66
Sophus::LieGroupTests::productTest
bool productTest()
Definition: tests.hpp:195
Sophus::SO2
SO2 using default storage; derived from SO2Base.
Definition: so2.hpp:19
Sophus::LieGroupTests::LieGroup
LieGroup_ LieGroup
Definition: tests.hpp:23
SOPHUS_TEST_APPROX
#define SOPHUS_TEST_APPROX(passed, left, right, thr,...)
Definition: test_macros.hpp:96
Sophus::SO3
SO3 using default storage; derived from SO3Base.
Definition: so3.hpp:19
test_macros.hpp
Sophus::LieGroupTests::additionalDerivativeTest
enable_if_t< std::is_same< G, Sophus::SO2< Scalar > >::value||std::is_same< G, Sophus::SO3< Scalar > >::value||std::is_same< G, Sophus::SE2< Scalar > >::value||std::is_same< G, Sophus::SE3< Scalar > >::value, bool > additionalDerivativeTest()
Definition: tests.hpp:140
Sophus::LieGroupTests::num_parameters
static constexpr int num_parameters
Definition: tests.hpp:34
Sophus::setElementAt
void setElementAt(T &p, Scalar value, int i)
Definition: types.hpp:180
Sophus::iterativeMean
optional< typename SequenceContainer::value_type > iterativeMean(SequenceContainer const &foo_Ts_bar, int max_num_iterations)
Definition: average.hpp:24
Sophus::setToZero
void setToZero(T &p)
Definition: types.hpp:172
Sophus::LieGroupTests::N
static constexpr int N
Definition: tests.hpp:32
Sophus::LieGroupTests::tangent_vec_
std::vector< Tangent, Eigen::aligned_allocator< Tangent > > tangent_vec_
Definition: tests.hpp:503
Sophus::Vector3
Vector< Scalar, 3, Options > Vector3
Definition: types.hpp:21
Sophus::Constants::epsilonSqrt
static SOPHUS_FUNC Scalar epsilonSqrt()
Definition: common.hpp:149
Sophus::enable_if_t
typename std::enable_if< B, T >::type enable_if_t
Definition: common.hpp:221
Sophus::LieGroupTests::LieGroupTests
LieGroupTests(std::vector< LieGroup, Eigen::aligned_allocator< LieGroup >> const &group_vec, std::vector< Tangent, Eigen::aligned_allocator< Tangent >> const &tangent_vec, std::vector< Point, Eigen::aligned_allocator< Point >> const &point_vec)
Definition: tests.hpp:36
Sophus::Vector2
Vector< Scalar, 2, Options > Vector2
Definition: types.hpp:16
Sophus::LieGroupTests::DoF
static constexpr int DoF
Definition: tests.hpp:33
Sophus::LieGroupTests::veeHatTest
bool veeHatTest()
Definition: tests.hpp:304
Sophus::LieGroupTests
Definition: tests.hpp:21
Sophus::LieGroupTests::expLogTest
bool expLogTest()
Definition: tests.hpp:209
Sophus::LieGroupTests::newDeleteSmokeTest
bool newDeleteSmokeTest()
Definition: tests.hpp:314
Sophus::LieGroupTests::HomogeneousPoint
typename LieGroup::HomogeneousPoint HomogeneousPoint
Definition: tests.hpp:28
SOPHUS_TEST
#define SOPHUS_TEST(passed, condition,...)
Definition: test_macros.hpp:53
Sophus::LieGroupTests::doesLargeTestSetPass
bool doesLargeTestSetPass()
Definition: tests.hpp:476
Sophus::LieGroupTests::Scalar
typename LieGroup::Scalar Scalar
Definition: tests.hpp:24
Sophus::curveNumDiff
auto curveNumDiff(Fn curve, Scalar t, Scalar h=Constants< Scalar >::epsilonSqrt()) -> decltype(details::Curve< Scalar >::num_diff(std::move(curve), t, h))
Definition: num_diff.hpp:72
Sophus::interpolate
enable_if_t< interp_details::Traits< G >::supported, G > interpolate(G const &foo_T_bar, G const &foo_T_baz, Scalar2 p=Scalar2(0.5f))
Definition: interpolate.hpp:27
interpolate.hpp
Sophus::interp_details::Traits
Definition: interpolate_details.hpp:17
Sophus::LieGroupTests::kSmallEps
const Scalar kSmallEps
Definition: tests.hpp:487
Sophus::LieGroupTests::point_vec_
std::vector< Point, Eigen::aligned_allocator< Point > > point_vec_
Definition: tests.hpp:504
Sophus::LieGroupTests::lineActionTest
bool lineActionTest()
Definition: tests.hpp:261
Sophus::LieGroupTests::ConstPointMap
Eigen::Map< const Point > ConstPointMap
Definition: tests.hpp:29
Sophus::Constants::epsilon
static SOPHUS_FUNC Scalar epsilon()
Definition: common.hpp:147
Sophus::Matrix
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:41
Sophus
Definition: average.hpp:17
Sophus::LieGroupTests::derivativeTest
bool derivativeTest()
Definition: tests.hpp:113
Sophus::LieGroupTests::Line
typename LieGroup::Line Line
Definition: tests.hpp:30
Sophus::SE2
SE2 using default storage; derived from SE2Base.
Definition: se2.hpp:11
Sophus::LieGroupTests::doAllTestsPass
enable_if_t<!std::is_floating_point< S >::value, bool > doAllTestsPass()
Definition: tests.hpp:457
Sophus::optional
Definition: common.hpp:185
Sophus::LieGroupTests::adjointTest
bool adjointTest()
Definition: tests.hpp:46
Sophus::transpose
auto transpose(T const &p) -> decltype(details::Transpose< T >::impl(T()))
Definition: types.hpp:195
Sophus::LieGroupTests::map
Eigen::Matrix< Scalar, N - 1, 1 > map(Eigen::Matrix< Scalar, N, N > const &T, Eigen::Matrix< Scalar, N - 1, 1 > const &p)
Definition: tests.hpp:490
Sophus::SE3
SE3 using default storage; derived from SE3Base.
Definition: se3.hpp:11
Sophus::LieGroupTests::doAllTestsPass
enable_if_t< std::is_floating_point< S >::value, bool > doAllTestsPass()
Definition: tests.hpp:452
Sophus::LieGroupTests::doesSmallTestSetPass
bool doesSmallTestSetPass()
Definition: tests.hpp:462
Sophus::LieGroupTests::group_vec_
std::vector< LieGroup, Eigen::aligned_allocator< LieGroup > > group_vec_
Definition: tests.hpp:502
Sophus::LieGroupTests::additionalDerivativeTest
enable_if_t<!std::is_same< G, Sophus::SO2< Scalar > >::value &&!std::is_same< G, Sophus::SO3< Scalar > >::value &&!std::is_same< G, Sophus::SE2< Scalar > >::value &&!std::is_same< G, Sophus::SE3< Scalar > >::value, bool > additionalDerivativeTest()
Definition: tests.hpp:191
SOPHUS_TEST_EQUAL
#define SOPHUS_TEST_EQUAL(passed, left, right,...)
Definition: test_macros.hpp:66
Sophus::Constants::pi
static SOPHUS_FUNC Scalar pi()
Definition: common.hpp:154
Sophus::getTestSE3s
std::vector< SE3< Scalar >, Eigen::aligned_allocator< SE3< Scalar > > > getTestSE3s()
Definition: tests.hpp:508
SOPHUS_TEST_NEQ
#define SOPHUS_TEST_NEQ(passed, left, right,...)
Definition: test_macros.hpp:81
Sophus::LieGroupTests::Tangent
typename LieGroup::Tangent Tangent
Definition: tests.hpp:26
Sophus::LieGroupTests::Point
typename LieGroup::Point Point
Definition: tests.hpp:27
average.hpp
Sophus::LieGroupTests::Transformation
typename LieGroup::Transformation Transformation
Definition: tests.hpp:25
Sophus::Vector
Eigen::Matrix< Scalar, M, 1, Options > Vector
Definition: types.hpp:13
Sophus::LieGroupTests::map
Eigen::Matrix< Scalar, N, 1 > map(Eigen::Matrix< Scalar, N, N > const &T, Eigen::Matrix< Scalar, N, 1 > const &p)
Definition: tests.hpp:497
Sophus::LieGroupTests::Adjoint
typename LieGroup::Adjoint Adjoint
Definition: tests.hpp:31
Sophus::LieGroupTests::interpolateAndMeanTest
bool interpolateAndMeanTest()
Definition: tests.hpp:323
num_diff.hpp
Sophus::average
enable_if_t< std::is_same< typename SequenceContainer::value_type, SO2< Scalar > >::value, optional< typename SequenceContainer::value_type > > average(SequenceContainer const &foo_Ts_bar)
Definition: average.hpp:70
Sophus::getTestSE2s
std::vector< SE2< T >, Eigen::aligned_allocator< SE2< T > > > getTestSE2s()
Definition: tests.hpp:547
Sophus::LieGroupTests::lieBracketTest
bool lieBracketTest()
Definition: tests.hpp:287
Sophus::LieGroupTests::groupActionTest
bool groupActionTest()
Definition: tests.hpp:233
Sophus::LieGroupTests::kSmallEpsSqrt
const Scalar kSmallEpsSqrt
Definition: tests.hpp:488

sophus
Author(s): Hauke Strasdat
autogenerated on Wed Mar 2 2022 01:01:48