MRPT  2.0.4
CPose3D_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
12 #include <mrpt/math/num_jacobian.h>
13 #include <mrpt/poses/CPoint3D.h>
14 #include <mrpt/poses/CPose2D.h>
15 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/poses/Lie/SE.h>
17 #include <Eigen/Dense>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 using namespace mrpt::math;
22 using namespace std;
23 
24 template class mrpt::CTraitsTest<CPose3D>;
25 
26 class Pose3DTests : public ::testing::Test
27 {
28  protected:
29  void SetUp() override {}
30  void TearDown() override {}
31  void test_inverse(const CPose3D& p1)
32  {
33  const auto HM = p1.getHomogeneousMatrixVal<CMatrixDouble44>();
34  CMatrixDouble44 HMi;
36 
37  auto I4 = CMatrixDouble44::Identity();
38 
40  (HM.asEigen() * HMi.asEigen() - I4.asEigen()).array().abs().sum(),
41  0, 1e-3)
42  << "HM:\n"
43  << HM << "inv(HM):\n"
44  << HMi << "inv(HM)*HM:\n"
45  << (HM * HMi) << endl;
46 
47  CPose3D p1_inv_inv = p1;
48 
49  p1_inv_inv.inverse();
50  const auto HMi_from_p1_inv =
52 
53  p1_inv_inv.inverse();
54 
56  (p1.asVectorVal() - p1_inv_inv.asVectorVal())
57  .asEigen()
58  .array()
59  .abs()
60  .sum(),
61  0, 1e-3)
62  << "p1: " << p1 << "p1_inv_inv: " << p1_inv_inv << endl;
63 
64  EXPECT_NEAR((HMi_from_p1_inv - HMi).sum_abs(), 0, 1e-4)
65  << "HMi_from_p1_inv:\n"
66  << HMi_from_p1_inv << "HMi:\n"
67  << HMi << endl;
68  }
69 
70  void test_compose(const CPose3D& p1, const CPose3D& p2)
71  {
72  const CPose3D p1_c_p2 = p1 + p2;
73  const CPose3D p1_i_p2 = p1 - p2;
74 
75  const CPose3D p1_c_p2_i_p2 = p1_c_p2 - p1; // should be -> p2
76  const CPose3D p2_c_p1_i_p2 = p2 + p1_i_p2; // Should be -> p1
77 
79  0,
80  (p1_c_p2_i_p2.asVectorVal() - p2.asVectorVal())
81  .asEigen()
82  .array()
83  .abs()
84  .sum(),
85  1e-5)
86  << "p2 : " << p2 << endl
87  << "p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl;
88 
90  0,
91  (p2_c_p1_i_p2.asVectorVal() - p1.asVectorVal())
92  .asEigen()
93  .array()
94  .abs()
95  .sum(),
96  1e-5)
97  << "p1 : " << p1 << endl
98  << "p2 : " << p2 << endl
99  << "p2 matrix : " << endl
100  << p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
101  << "p1_i_p2 : " << p1_i_p2 << endl
102  << "p1_i_p2 matrix: " << endl
103  << p1_i_p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
104  << "p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl;
105 
106  // Test + operator: trg new var
107  {
108  CPose3D C = p1;
109  CPose3D A = C + p2;
110  EXPECT_NEAR(
111  0,
112  (A.asVectorVal() - p1_c_p2.asVectorVal())
113  .asEigen()
114  .array()
115  .abs()
116  .sum(),
117  1e-6);
118  }
119  // Test + operator: trg same var
120  {
121  CPose3D A = p1;
122  A = A + p2;
123  EXPECT_NEAR(
124  0,
125  (A.asVectorVal() - p1_c_p2.asVectorVal())
126  .asEigen()
127  .array()
128  .abs()
129  .sum(),
130  1e-6);
131  }
132  // Test =+ operator
133  {
134  CPose3D A = p1;
135  A += p2;
136  EXPECT_NEAR(
137  0,
138  (A.asVectorVal() - p1_c_p2.asVectorVal())
139  .asEigen()
140  .array()
141  .abs()
142  .sum(),
143  1e-6);
144  }
145  }
146 
147  void test_to_from_2d(double x, double y, double phi)
148  {
149  const CPose2D p2d = CPose2D(x, y, phi);
150  const CPose3D p3d = CPose3D(p2d);
151 
152  const CPose2D p2d_bis = CPose2D(p3d);
153 
154  EXPECT_DOUBLE_EQ(p2d.x(), p2d_bis.x()) << "p2d: " << p2d << endl;
155  EXPECT_DOUBLE_EQ(p2d.y(), p2d_bis.y()) << "p2d: " << p2d << endl;
156  EXPECT_DOUBLE_EQ(p2d.phi(), p2d_bis.phi()) << "p2d: " << p2d << endl;
157 
158  EXPECT_DOUBLE_EQ(p2d.phi(), p3d.yaw()) << "p2d: " << p2d << endl;
159  }
160 
161  void test_composeFrom(const CPose3D& p1, const CPose3D& p2)
162  {
163  const CPose3D p1_plus_p2 = p1 + p2;
164 
165  {
166  CPose3D p1_plus_p2bis;
167  p1_plus_p2bis.composeFrom(p1, p2);
168 
169  EXPECT_NEAR(
170  0,
171  (p1_plus_p2bis.asVectorVal() - p1_plus_p2.asVectorVal())
172  .asEigen()
173  .array()
174  .abs()
175  .sum(),
176  1e-5)
177  << "p2 : " << p2 << endl
178  << "p1 : " << p1 << endl
179  << "p1_plus_p2 : " << p1_plus_p2 << endl
180  << "p1_plus_p2bis : " << p1_plus_p2bis << endl;
181  }
182 
183  {
184  CPose3D p1_plus_p2bis = p1;
185  p1_plus_p2bis.composeFrom(p1_plus_p2bis, p2);
186 
187  EXPECT_NEAR(
188  0,
189  (p1_plus_p2bis.asVectorVal() - p1_plus_p2.asVectorVal())
190  .asEigen()
191  .array()
192  .abs()
193  .sum(),
194  1e-5)
195  << "p2 : " << p2 << endl
196  << "p1 : " << p1 << endl
197  << "p1_plus_p2 : " << p1_plus_p2 << endl
198  << "p1_plus_p2bis : " << p1_plus_p2bis << endl;
199  }
200 
201  {
202  CPose3D p1_plus_p2bis = p2;
203  p1_plus_p2bis.composeFrom(p1, p1_plus_p2bis);
204 
205  EXPECT_NEAR(
206  0,
207  (p1_plus_p2bis.asVectorVal() - p1_plus_p2.asVectorVal())
208  .asEigen()
209  .array()
210  .abs()
211  .sum(),
212  1e-5)
213  << "p2 : " << p2 << endl
214  << "p1 : " << p1 << endl
215  << "p1_plus_p2 : " << p1_plus_p2 << endl
216  << "p1_plus_p2bis : " << p1_plus_p2bis << endl;
217  }
218  }
219 
220  void test_composePoint(const CPose3D& p1, double x, double y, double z)
221  {
222  const CPoint3D p(x, y, z);
223  CPoint3D p1_plus_p = p1 + p;
224 
225  CPoint3D p1_plus_p2;
226  p1.composePoint(
227  p.x(), p.y(), p.z(), p1_plus_p2.x(), p1_plus_p2.y(),
228  p1_plus_p2.z());
229 
230  EXPECT_NEAR(
231  0,
232  (p1_plus_p2.asVectorVal() - p1_plus_p.asVectorVal())
233  .asEigen()
234  .array()
235  .abs()
236  .sum(),
237  1e-5);
238 
239  // Repeat using same input/output variables:
240  {
241  double lx = p.x();
242  double ly = p.y();
243  double lz = p.z();
244 
245  p1.composePoint(lx, ly, lz, lx, ly, lz);
246  EXPECT_NEAR(
247  0,
248  std::abs(lx - p1_plus_p.x()) + std::abs(ly - p1_plus_p.y()) +
249  std::abs(lz - p1_plus_p.z()),
250  1e-5);
251  }
252 
253  // Inverse:
254  CPoint3D p_recov = p1_plus_p - p1;
255  CPoint3D p_recov2;
256  p1.inverseComposePoint(
257  p1_plus_p.x(), p1_plus_p.y(), p1_plus_p.z(), p_recov2.x(),
258  p_recov2.y(), p_recov2.z());
259 
260  EXPECT_NEAR(
261  0,
262  (p_recov2.asVectorVal() - p_recov.asVectorVal())
263  .asEigen()
264  .array()
265  .abs()
266  .sum(),
267  1e-5);
268 
269  EXPECT_NEAR(
270  0,
271  (p.asVectorVal() - p_recov.asVectorVal())
272  .asEigen()
273  .array()
274  .abs()
275  .sum(),
276  1e-5);
277  }
278 
279  static void func_compose_point(
280  const CVectorFixedDouble<6 + 3>& x,
281  [[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
282  {
283  CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
284  const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
285  const CPoint3D pp = q + p;
286  for (int i = 0; i < 3; i++) Y[i] = pp[i];
287  }
288 
290  const CVectorFixedDouble<6 + 3>& x,
291  [[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
292  {
293  CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
294  const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
295  const CPoint3D pp = p - q;
296  Y[0] = pp.x();
297  Y[1] = pp.y();
298  Y[2] = pp.z();
299  }
300 
302  const CPose3D& p1, double x, double y, double z, bool use_aprox = false)
303  {
304  const CPoint3D p(x, y, z);
305 
306  CMatrixFixed<double, 3, 3> df_dpoint;
308 
309  TPoint3D pp;
310  p1.composePoint(
311  x, y, z, pp.x, pp.y, pp.z, df_dpoint, df_dpose, std::nullopt,
312  use_aprox);
313 
314  // Numerical approx:
317  {
319  for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
320  x_mean[6 + 0] = x;
321  x_mean[6 + 1] = y;
322  x_mean[6 + 2] = z;
323 
324  double DUMMY = 0;
326  x_incrs.fill(1e-7);
327  CMatrixDouble numJacobs;
329  x_mean,
330  std::function<void(
331  const CVectorFixedDouble<6 + 3>& x, const double& dummy,
332  CVectorFixedDouble<3>& Y)>(&func_compose_point),
333  x_incrs, DUMMY, numJacobs);
334 
335  num_df_dpose = numJacobs.block<3, 6>(0, 0);
336  num_df_dpoint = numJacobs.block<3, 3>(0, 6);
337  }
338 
339  const double max_error = use_aprox ? 0.1 : 3e-3;
340 
341  EXPECT_NEAR(0, (df_dpoint - num_df_dpoint).sum_abs(), max_error)
342  << "p1: " << p1 << endl
343  << "p: " << p << endl
344  << "Numeric approximation of df_dpoint: " << endl
345  << num_df_dpoint << endl
346  << "Implemented method: " << endl
347  << df_dpoint << endl
348  << "Error: " << endl
349  << df_dpoint - num_df_dpoint << endl;
350 
351  EXPECT_NEAR(
352  0,
353  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
354  max_error)
355  << "p1: " << p1 << endl
356  << "p: " << p << endl
357  << "Numeric approximation of df_dpose: " << endl
358  << num_df_dpose.asEigen() << endl
359  << "Implemented method: " << endl
360  << df_dpose.asEigen() << endl
361  << "Error: " << endl
362  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
363  }
364 
365  void test_ExpLnEqual(const CPose3D& p1)
366  {
367  const CPose3D p2 = Lie::SE<3>::exp(Lie::SE<3>::log(p1));
368  EXPECT_NEAR((p1.asVectorVal() - p2.asVectorVal()).sum_abs(), 0, 1e-5)
369  << "p1: " << p1 << endl;
370  }
371 
373  const CPose3D& p1, double x, double y, double z)
374  {
375  const CPoint3D p(x, y, z);
376 
377  CMatrixFixed<double, 3, 3> df_dpoint;
379 
380  TPoint3D pp;
381  p1.inverseComposePoint(x, y, z, pp.x, pp.y, pp.z, df_dpoint, df_dpose);
382 
383  // Numerical approx:
386  {
388  for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
389  x_mean[6 + 0] = x;
390  x_mean[6 + 1] = y;
391  x_mean[6 + 2] = z;
392 
393  double DUMMY = 0;
395  x_incrs.fill(1e-7);
396  CMatrixDouble numJacobs;
398  x_mean,
399  std::function<void(
400  const CVectorFixedDouble<6 + 3>& x, const double& dummy,
401  CVectorFixedDouble<3>& Y)>(&func_inv_compose_point),
402  x_incrs, DUMMY, numJacobs);
403 
404  num_df_dpose = numJacobs.block<3, 6>(0, 0);
405  num_df_dpoint = numJacobs.block<3, 3>(0, 6);
406  }
407 
408  EXPECT_NEAR(
409  0, (df_dpoint - num_df_dpoint).asEigen().array().abs().sum(), 3e-3)
410  << "p1: " << p1 << endl
411  << "p: " << p << endl
412  << "Numeric approximation of df_dpoint: " << endl
413  << num_df_dpoint << endl
414  << "Implemented method: " << endl
415  << df_dpoint << endl
416  << "Error: " << endl
417  << df_dpoint - num_df_dpoint << endl;
418 
419  EXPECT_NEAR(
420  0,
421  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
422  3e-3)
423  << "p1: " << p1 << endl
424  << "p: " << p << endl
425  << "Numeric approximation of df_dpose: " << endl
426  << num_df_dpose.asEigen() << endl
427  << "Implemented method: " << endl
428  << df_dpose.asEigen() << endl
429  << "Error: " << endl
430  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
431  }
432 
433  void test_default_values(const CPose3D& p, const std::string& label)
434  {
435  EXPECT_EQ(p.x(), 0);
436  EXPECT_EQ(p.y(), 0);
437  EXPECT_EQ(p.z(), 0);
438  EXPECT_EQ(p.yaw(), 0);
439  EXPECT_EQ(p.pitch(), 0);
440  EXPECT_EQ(p.roll(), 0);
441  for (size_t i = 0; i < 4; i++)
442  for (size_t j = 0; j < 4; j++)
443  EXPECT_NEAR(
445  i == j ? 1.0 : 0.0, 1e-8)
446  << "Failed for (i,j)=" << i << "," << j << endl
447  << "Matrix is: " << endl
449  << "case was: " << label << endl;
450  }
451 
453  const CVectorFixedDouble<6>& x, const CVectorFixedDouble<3>& P,
455  {
456  CPose3D q = Lie::SE<3>::exp(x);
457  const CPoint3D p(P[0], P[1], P[2]);
458  const CPoint3D pp = q + p;
459  for (int i = 0; i < 3; i++) Y[i] = pp[i];
460  }
461 
463  const CVectorFixedDouble<6>& x, const CVectorFixedDouble<3>& P,
465  {
466  CPose3D q = Lie::SE<3>::exp(x);
467  const CPoint3D p(P[0], P[1], P[2]);
468  const CPoint3D pp = p - q;
469  for (int i = 0; i < 3; i++) Y[i] = pp[i];
470  }
471 
472  void test_composePointJacob_se3(const CPose3D& p, const TPoint3D x_l)
473  {
475 
476  TPoint3D pp;
477  p.composePoint(
478  x_l.x, x_l.y, x_l.z, pp.x, pp.y, pp.z, std::nullopt, std::nullopt,
479  df_dse3);
480 
481  // Numerical approx:
483  {
484  CVectorFixedDouble<6> x_mean;
485  for (int i = 0; i < 6; i++) x_mean[i] = 0;
486 
488  for (int i = 0; i < 3; i++) P[i] = pp[i];
489 
490  CVectorFixedDouble<6> x_incrs;
491  x_incrs.fill(1e-9);
493  x_mean,
494  std::function<void(
495  const CVectorFixedDouble<6>& x,
497  &func_compose_point_se3),
498  x_incrs, P, num_df_dse3);
499  }
500 
501  EXPECT_NEAR(
502  0, (df_dse3.asEigen() - num_df_dse3.asEigen()).array().abs().sum(),
503  3e-3)
504  << "p: " << p << endl
505  << "x_l: " << x_l << endl
506  << "Numeric approximation of df_dse3: " << endl
507  << num_df_dse3.asEigen() << endl
508  << "Implemented method: " << endl
509  << df_dse3.asEigen() << endl
510  << "Error: " << endl
511  << df_dse3.asEigen() - num_df_dse3.asEigen() << endl;
512  }
513 
515  {
517 
518  TPoint3D pp;
520  x_g.x, x_g.y, x_g.z, pp.x, pp.y, pp.z, std::nullopt, std::nullopt,
521  df_dse3);
522 
523  // Numerical approx:
525  {
526  CVectorFixedDouble<6> x_mean;
527  for (int i = 0; i < 6; i++) x_mean[i] = 0;
528 
530  for (int i = 0; i < 3; i++) P[i] = pp[i];
531 
532  CVectorFixedDouble<6> x_incrs;
533  x_incrs.fill(1e-9);
535  x_mean,
536  std::function<void(
537  const CVectorFixedDouble<6>& x,
539  &func_invcompose_point_se3),
540  x_incrs, P, num_df_dse3);
541  }
542 
543  EXPECT_NEAR(
544  0, (df_dse3.asEigen() - num_df_dse3.asEigen()).array().abs().sum(),
545  3e-3)
546  << "p: " << p << endl
547  << "x_g: " << x_g << endl
548  << "Numeric approximation of df_dse3: " << endl
549  << num_df_dse3.asEigen() << endl
550  << "Implemented method: " << endl
551  << df_dse3.asEigen() << endl
552  << "Error: " << endl
553  << df_dse3.asEigen() - num_df_dse3.asEigen() << endl;
554  }
555 
556  static void func_jacob_expe_e(
557  const CVectorFixedDouble<6>& x, [[maybe_unused]] const double& dummy,
559  {
560  const CPose3D p = Lie::SE<3>::exp(x);
561  // const CMatrixDouble44 R =
562  // p.getHomogeneousMatrixVal<CMatrixDouble44>();
563  p.getAs12Vector(Y);
564  }
565 
566  // Check dexp(e)_de
568  {
569  CVectorFixedDouble<6> x_mean;
570  for (int i = 0; i < 6; i++) x_mean[i] = 0;
571 
572  const double& dummy = 0.;
573  CVectorFixedDouble<6> x_incrs;
574  x_incrs.fill(1e-9);
575  CMatrixDouble numJacobs;
577  x_mean,
578  std::function<void(
579  const CVectorFixedDouble<6>& x, const double& dummy,
580  CVectorFixedDouble<12>& Y)>(&func_jacob_expe_e),
581  x_incrs, dummy, numJacobs);
582 
583  // Theoretical matrix:
584  // [ 0 -[e1]_x ]
585  // [ 0 -[e2]_x ]
586  // [ 0 -[e3]_x ]
587  // [ I_3 0 ]
588  double vals[12 * 6] = {
589  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0,
590 
591  0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
592 
593  0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0,
594 
595  1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0};
597 
598  EXPECT_NEAR(
599  (numJacobs.asEigen() - M.asEigen()).array().abs().maxCoeff(), 0,
600  1e-5)
601  << "M:\n"
602  << M.asEigen() << "numJacobs:\n"
603  << numJacobs << "\n";
604  }
605 
606  static void func_jacob_LnT_T(
607  const CVectorFixedDouble<12>& x, [[maybe_unused]] const double& dummy,
609  {
610  CPose3D p;
611 
612  p.setFrom12Vector(x);
613  // ensure 3x3 rot vector is orthonormal (Sophus complains otherwise):
614  auto R = p.getRotationMatrix();
615  const auto Rsvd =
616  R.asEigen().jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
617  R = Rsvd.matrixU() * Rsvd.matrixV().transpose();
618  p.setRotationMatrix(R);
619 
620  Y = Lie::SE<3>::log(p);
621  }
622 
623  // Jacobian of Ln(T) wrt T
624  void check_jacob_LnT_T(const CPose3D& p)
625  {
626  const auto theor_jacob = Lie::SE<3>::jacob_dlogv_dv(p);
627 
628  CMatrixDouble numJacobs;
629  {
630  CVectorFixedDouble<12> x_mean;
631  p.getAs12Vector(x_mean);
632 
633  const double& dummy = 0.;
634  CVectorFixedDouble<12> x_incrs;
635  x_incrs.fill(1e-6);
637  x_mean,
638  std::function<void(
639  const CVectorFixedDouble<12>& x, const double& dummy,
640  CVectorFixedDouble<6>& Y)>(&func_jacob_LnT_T),
641  x_incrs, dummy, numJacobs);
642  }
643 
644  EXPECT_NEAR(
645  (numJacobs.asEigen() - theor_jacob.asEigen()).array().abs().sum(),
646  0, 1e-3)
647  << "Pose: " << p << endl
648  << "Pose matrix:\n"
649  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
650  << numJacobs << endl
651  << "Theor. Jacob:\n"
652  << theor_jacob.asEigen() << endl
653  << "ERR:\n"
654  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
655  }
656 
657  static void func_jacob_expe_D(
658  const CVectorFixedDouble<6>& eps, const CPose3D& D,
660  {
661  const CPose3D incr = Lie::SE<3>::exp(eps);
662  const CPose3D expe_D = incr + D;
663  expe_D.getAs12Vector(Y);
664  }
665 
666  // Test Jacobian: d exp(e)*D / d e
667  // 10.3.3 in tech report
669  {
670  const auto theor_jacob = Lie::SE<3>::jacob_dexpeD_de(p);
671 
672  CMatrixDouble numJacobs;
673  {
674  CVectorFixedDouble<6> x_mean;
675  x_mean.setZero();
676 
677  CVectorFixedDouble<6> x_incrs;
678  x_incrs.fill(1e-6);
680  x_mean,
681  std::function<void(
682  const CVectorFixedDouble<6>& eps, const CPose3D& D,
683  CVectorFixedDouble<12>& Y)>(&func_jacob_expe_D),
684  x_incrs, p, numJacobs);
685  }
686 
687  EXPECT_NEAR(
688  (numJacobs.asEigen() - theor_jacob.asEigen())
689  .array()
690  .abs()
691  .maxCoeff(),
692  0, 1e-3)
693  << "Pose: " << p << endl
694  << "Pose matrix:\n"
695  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
696  << numJacobs << endl
697  << "Theor. Jacob:\n"
698  << theor_jacob.asEigen() << endl
699  << "ERR:\n"
700  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
701  }
702 
703  static void func_jacob_D_expe(
704  const CVectorFixedDouble<6>& eps, const CPose3D& D,
706  {
707  const CPose3D incr = Lie::SE<3>::exp(eps);
708  const CPose3D expe_D = D + incr;
709  expe_D.getAs12Vector(Y);
710  }
711 
712  // Test Jacobian: d D*exp(e) / d e
713  // 10.3.4 in tech report
715  {
716  const auto theor_jacob = Lie::SE<3>::jacob_dDexpe_de(p);
717 
718  CMatrixDouble numJacobs;
719  {
720  CVectorFixedDouble<6> x_mean;
721  x_mean.setZero();
722 
723  CVectorFixedDouble<6> x_incrs;
724  x_incrs.fill(1e-6);
726  x_mean,
727  std::function<void(
728  const CVectorFixedDouble<6>& eps, const CPose3D& D,
729  CVectorFixedDouble<12>& Y)>(&func_jacob_D_expe),
730  x_incrs, p, numJacobs);
731  }
732 
733  EXPECT_NEAR(
734  (numJacobs.asEigen() - theor_jacob.asEigen())
735  .array()
736  .abs()
737  .maxCoeff(),
738  0, 1e-3)
739  << "Pose: " << p << endl
740  << "Pose matrix:\n"
741  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
742  << numJacobs << endl
743  << "Theor. Jacob:\n"
744  << theor_jacob.asEigen() << endl
745  << "ERR:\n"
746  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
747  }
748 
750  {
752  };
753 
754  static void func_jacob_Aexpe_D(
755  const CVectorFixedDouble<6>& eps,
757  {
758  const CPose3D incr = Lie::SE<3>::exp(eps);
759  const CPose3D res = params.A + incr + params.D;
760  res.getAs12Vector(Y);
761  }
762 
763  // Test Jacobian: d A*exp(e)*D / d e
764  // 10.3.7 in tech report
765  // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
766  void test_Jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D)
767  {
768  const auto theor_jacob = Lie::SE<3>::jacob_dAexpeD_de(A, D);
769 
770  CMatrixDouble numJacobs;
771  {
772  CVectorFixedDouble<6> x_mean;
773  x_mean.setZero();
774 
776  params.A = A;
777  params.D = D;
778  CVectorFixedDouble<6> x_incrs;
779  x_incrs.fill(1e-6);
781  x_mean,
782  std::function<void(
783  const CVectorFixedDouble<6>& eps,
785  CVectorFixedDouble<12>& Y)>(&func_jacob_Aexpe_D),
786  x_incrs, params, numJacobs);
787  }
788 
789  EXPECT_NEAR(
790  (numJacobs.asEigen() - theor_jacob.asEigen())
791  .array()
792  .abs()
793  .maxCoeff(),
794  0, 1e-3)
795  << "Pose A: " << A << endl
796  << "Pose D: " << D << endl
797  << "Num. Jacob:\n"
798  << numJacobs << endl
799  << "Theor. Jacob:\n"
800  << theor_jacob.asEigen() << endl
801  << "ERR:\n"
802  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
803  }
804 };
805 
806 // Elemental tests:
807 TEST_F(Pose3DTests, DefaultValues)
808 {
809  {
810  CPose3D p;
811  test_default_values(p, "Default");
812  }
813  {
814  CPose3D p2;
815  CPose3D p = p2;
816  test_default_values(p, "p=p2");
817  }
818  {
819  CPose3D p1, p2;
820  test_default_values(p1 + p2, "p1+p2");
821  CPose3D p = p1 + p2;
822  test_default_values(p, "p=p1+p2");
823  }
824  {
825  CPose3D p1, p2;
826  CPose3D p = p1 - p2;
827  test_default_values(p, "p1-p2");
828  }
829 }
830 
831 TEST_F(Pose3DTests, Initialization)
832 {
833  CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
834  EXPECT_NEAR(p.x(), 1, 1e-7);
835  EXPECT_NEAR(p.y(), 2, 1e-7);
836  EXPECT_NEAR(p.z(), 3, 1e-7);
837  EXPECT_NEAR(p.yaw(), 0.2, 1e-7);
838  EXPECT_NEAR(p.pitch(), 0.3, 1e-7);
839  EXPECT_NEAR(p.roll(), 0.4, 1e-7);
840 }
841 
842 TEST_F(Pose3DTests, OperatorBracket)
843 {
844  CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
845  EXPECT_NEAR(p[0], 1, 1e-7);
846  EXPECT_NEAR(p[1], 2, 1e-7);
847  EXPECT_NEAR(p[2], 3, 1e-7);
848  EXPECT_NEAR(p[3], 0.2, 1e-7);
849  EXPECT_NEAR(p[4], 0.3, 1e-7);
850  EXPECT_NEAR(p[5], 0.4, 1e-7);
851 }
852 
853 // List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in
854 // degrees)
855 static const std::vector<mrpt::poses::CPose3D> ptc = {
856  {.0, .0, .0, .0_deg, .0_deg, .0_deg},
857  {1.0, 2.0, 3.0, .0_deg, .0_deg, .0_deg},
858  {1.0, 2.0, 3.0, 10.0_deg, .0_deg, .0_deg},
859  {1.0, 2.0, 3.0, .0_deg, 1.0_deg, .0_deg},
860  {1.0, 2.0, 3.0, .0_deg, .0_deg, 1.0_deg},
861  {1.0, 2.0, 3.0, 80.0_deg, 5.0_deg, 5.0_deg},
862  {1.0, 2.0, 3.0, -20.0_deg, -30.0_deg, -40.0_deg},
863  {1.0, 2.0, 3.0, -45.0_deg, 10.0_deg, 70.0_deg},
864  {1.0, 2.0, 3.0, 40.0_deg, -5.0_deg, 25.0_deg},
865  {1.0, 2.0, 3.0, 40.0_deg, 20.0_deg, -15.0_deg},
866  {-6.0, 2.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
867  {6.0, -5.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
868  {6.0, 2.0, -9.0, 40.0_deg, 20.0_deg, 15.0_deg},
869  {0.0, 8.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
870  {1.0, 0.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
871  {1.0, 8.0, 0.0, -45.0_deg, 10.0_deg, 70.0_deg}};
872 
873 // More complex tests:
874 TEST_F(Pose3DTests, InverseHM)
875 {
876  for (const auto& p : ptc) test_inverse(p);
877 }
878 
880 {
881  for (const auto& p1 : ptc)
882  for (const auto& p2 : ptc) test_compose(p1, p2);
883 }
884 TEST_F(Pose3DTests, composeFrom)
885 {
886  for (const auto& p1 : ptc)
887  for (const auto& p2 : ptc) test_composeFrom(p1, p2);
888 }
889 
890 TEST_F(Pose3DTests, ToFromCPose2D)
891 {
892  for (const auto& p : ptc) test_to_from_2d(p.x(), p.y(), p.yaw());
893 }
894 
895 TEST_F(Pose3DTests, ComposeAndInvComposeWithPoint)
896 {
897  for (const auto& p : ptc)
898  {
899  test_composePoint(p, 10, 11, 12);
900  test_composePoint(p, -5, 1, 2);
901  test_composePoint(p, 5, -1, 2);
902  test_composePoint(p, 5, 1, -2);
903  }
904 }
905 
906 TEST_F(Pose3DTests, ComposePointJacob)
907 {
908  for (const auto& p : ptc)
909  {
910  test_composePointJacob(p, 10, 11, 12);
911  test_composePointJacob(p, -5, 1, 2);
912  }
913 }
914 
915 TEST_F(Pose3DTests, ComposePointJacobApprox)
916 { // Test approximated Jacobians for very small rotations
917  test_composePointJacob(
918  mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg), 10, 11,
919  12, true);
920  test_composePointJacob(
921  mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.1_deg, 0.0_deg, 0.0_deg), 10, 11,
922  12, true);
923  test_composePointJacob(
924  mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.1_deg, 0.0_deg), 10, 11,
925  12, true);
926  test_composePointJacob(
927  mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.1_deg), 10, 11,
928  12, true);
929 }
930 
931 TEST_F(Pose3DTests, InvComposePointJacob)
932 {
933  for (const auto& p : ptc)
934  {
935  test_invComposePointJacob(p, 10, 11, 12);
936  test_invComposePointJacob(p, -5, 1, 2);
937  test_invComposePointJacob(p, 5, -1, 2);
938  test_invComposePointJacob(p, 5, 1, -2);
939  }
940 }
941 
942 TEST_F(Pose3DTests, ComposePointJacob_se3)
943 {
944  for (const auto& p : ptc)
945  {
946  test_composePointJacob_se3(p, TPoint3D(0, 0, 0));
947  test_composePointJacob_se3(p, TPoint3D(10, 11, 12));
948  test_composePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
949  }
950 }
951 TEST_F(Pose3DTests, InvComposePointJacob_se3)
952 {
953  for (const auto& p : ptc)
954  {
955  test_invComposePointJacob_se3(p, TPoint3D(0, 0, 0));
956  test_invComposePointJacob_se3(p, TPoint3D(10, 11, 12));
957  test_invComposePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
958  }
959 }
960 
961 TEST_F(Pose3DTests, ExpLnEqual)
962 {
963  for (const auto& p : ptc) test_ExpLnEqual(p);
964 }
965 
966 TEST_F(Pose3DTests, Jacob_dExpe_de_at_0) { check_jacob_expe_e_at_0(); }
967 TEST_F(Pose3DTests, Jacob_dLnT_dT)
968 {
969  check_jacob_LnT_T(mrpt::poses::CPose3D(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg));
970  // JL NOTE:
971  // This function cannot be properly tested numerically, since the logm()
972  // implementation
973  // is not generic and does NOT depends on all matrix entries, thus the
974  // numerical Jacobian
975  // contains entire columns of zeros, even if the theorethical doesn't.
976  // check_jacob_LnT_T(1.0,0,0, 0.0_deg,0.0_deg,0.0_deg ); ...
977 }
978 
979 TEST_F(Pose3DTests, Jacob_dexpeD_de)
980 {
981  for (const auto& p : ptc) test_Jacob_dexpeD_de(p);
982 }
983 
984 TEST_F(Pose3DTests, Jacob_dDexpe_de)
985 {
986  for (const auto& p : ptc) test_Jacob_dDexpe_de(p);
987 }
988 
989 TEST_F(Pose3DTests, Jacob_dAexpeD_de)
990 {
991  for (const auto& p1 : ptc)
992  for (const auto& p2 : ptc) test_Jacob_dAexpeD_de(p1, p2);
993 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
static void func_compose_point_se3(const CVectorFixedDouble< 6 > &x, const CVectorFixedDouble< 3 > &P, CVectorFixedDouble< 3 > &Y)
TEST_F(Pose3DTests, DefaultValues)
void test_invComposePointJacob_se3(const CPose3D &p, const TPoint3D x_g)
static void func_compose_point(const CVectorFixedDouble< 6+3 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 3 > &Y)
static const std::vector< mrpt::poses::CPose3D > ptc
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
void test_inverse(const CPose3D &p1)
void test_composePointJacob_se3(const CPose3D &p, const TPoint3D x_l)
void test_invComposePointJacob(const CPose3D &p1, double x, double y, double z)
void test_Jacob_dexpeD_de(const CPose3D &p)
static void func_jacob_expe_D(const CVectorFixedDouble< 6 > &eps, const CPose3D &D, CVectorFixedDouble< 12 > &Y)
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
void test_composePoint(const CPose3D &p1, double x, double y, double z)
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
mrpt::vision::TStereoCalibParams params
STL namespace.
static void func_jacob_expe_e(const CVectorFixedDouble< 6 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 12 > &Y)
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:584
void test_to_from_2d(double x, double y, double phi)
static void func_inv_compose_point(const CVectorFixedDouble< 6+3 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 3 > &Y)
static void func_jacob_Aexpe_D(const CVectorFixedDouble< 6 > &eps, const TParams_func_jacob_Aexpe_D &params, CVectorFixedDouble< 12 > &Y)
static void func_jacob_LnT_T(const CVectorFixedDouble< 12 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 6 > &Y)
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
Definition: CPose3D.h:237
void check_jacob_expe_e_at_0()
This base provides a set of functions for maths stuff.
void test_ExpLnEqual(const CPose3D &p1)
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:556
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
void test_Jacob_dAexpeD_de(const CPose3D &A, const CPose3D &D)
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:290
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
const double eps
void test_composePointJacob(const CPose3D &p1, double x, double y, double z, bool use_aprox=false)
static void func_invcompose_point_se3(const CVectorFixedDouble< 6 > &x, const CVectorFixedDouble< 3 > &P, CVectorFixedDouble< 3 > &Y)
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:31
void test_compose(const CPose3D &p1, const CPose3D &p2)
void setFrom12Vector(const ARRAYORVECTOR &vec12)
Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r...
Definition: CPose3D.h:499
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
void test_Jacob_dDexpe_de(const CPose3D &p)
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
void check_jacob_LnT_T(const CPose3D &p)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
static void func_jacob_D_expe(const CVectorFixedDouble< 6 > &eps, const CPose3D &D, CVectorFixedDouble< 12 > &Y)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
void test_composeFrom(const CPose3D &p1, const CPose3D &p2)
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:278
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
void getAs12Vector(ARRAYORVECTOR &vec12) const
Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 ...
Definition: CPose3D.h:523
void test_default_values(const CPose3D &p, const std::string &label)
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:266
void TearDown() override
void SetUp() override



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020