MRPT  1.9.9
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-2019, 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_FLOAT_EQ(p2d.x(), p2d_bis.x()) << "p2d: " << p2d << endl;
155  EXPECT_FLOAT_EQ(p2d.y(), p2d_bis.y()) << "p2d: " << p2d << endl;
156  EXPECT_FLOAT_EQ(p2d.phi(), p2d_bis.phi()) << "p2d: " << p2d << endl;
157 
158  EXPECT_FLOAT_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, const double& dummy,
282  {
283  MRPT_UNUSED_PARAM(dummy);
284  CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
285  const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
286  const CPoint3D pp = q + p;
287  for (int i = 0; i < 3; i++) Y[i] = pp[i];
288  }
289 
291  const CVectorFixedDouble<6 + 3>& x, const double& dummy,
293  {
294  MRPT_UNUSED_PARAM(dummy);
295  CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
296  const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
297  const CPoint3D pp = p - q;
298  Y[0] = pp.x();
299  Y[1] = pp.y();
300  Y[2] = pp.z();
301  }
302 
304  const CPose3D& p1, double x, double y, double z, bool use_aprox = false)
305  {
306  const CPoint3D p(x, y, z);
307 
308  CMatrixFixed<double, 3, 3> df_dpoint;
310 
311  TPoint3D pp;
312  p1.composePoint(
313  x, y, z, pp.x, pp.y, pp.z, df_dpoint, df_dpose, std::nullopt,
314  use_aprox);
315 
316  // Numerical approx:
319  {
321  for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
322  x_mean[6 + 0] = x;
323  x_mean[6 + 1] = y;
324  x_mean[6 + 2] = z;
325 
326  double DUMMY = 0;
328  x_incrs.fill(1e-7);
329  CMatrixDouble numJacobs;
331  x_mean,
332  std::function<void(
333  const CVectorFixedDouble<6 + 3>& x, const double& dummy,
334  CVectorFixedDouble<3>& Y)>(&func_compose_point),
335  x_incrs, DUMMY, numJacobs);
336 
337  num_df_dpose = numJacobs.block<3, 6>(0, 0);
338  num_df_dpoint = numJacobs.block<3, 3>(0, 6);
339  }
340 
341  const double max_error = use_aprox ? 0.1 : 3e-3;
342 
343  EXPECT_NEAR(0, (df_dpoint - num_df_dpoint).sum_abs(), max_error)
344  << "p1: " << p1 << endl
345  << "p: " << p << endl
346  << "Numeric approximation of df_dpoint: " << endl
347  << num_df_dpoint << endl
348  << "Implemented method: " << endl
349  << df_dpoint << endl
350  << "Error: " << endl
351  << df_dpoint - num_df_dpoint << endl;
352 
353  EXPECT_NEAR(
354  0,
355  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
356  max_error)
357  << "p1: " << p1 << endl
358  << "p: " << p << endl
359  << "Numeric approximation of df_dpose: " << endl
360  << num_df_dpose.asEigen() << endl
361  << "Implemented method: " << endl
362  << df_dpose.asEigen() << endl
363  << "Error: " << endl
364  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
365  }
366 
367  void test_ExpLnEqual(const CPose3D& p1)
368  {
369  const CPose3D p2 = Lie::SE<3>::exp(Lie::SE<3>::log(p1));
370  EXPECT_NEAR((p1.asVectorVal() - p2.asVectorVal()).sum_abs(), 0, 1e-5)
371  << "p1: " << p1 << endl;
372  }
373 
375  const CPose3D& p1, double x, double y, double z)
376  {
377  const CPoint3D p(x, y, z);
378 
379  CMatrixFixed<double, 3, 3> df_dpoint;
381 
382  TPoint3D pp;
383  p1.inverseComposePoint(x, y, z, pp.x, pp.y, pp.z, df_dpoint, df_dpose);
384 
385  // Numerical approx:
388  {
390  for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
391  x_mean[6 + 0] = x;
392  x_mean[6 + 1] = y;
393  x_mean[6 + 2] = z;
394 
395  double DUMMY = 0;
397  x_incrs.fill(1e-7);
398  CMatrixDouble numJacobs;
400  x_mean,
401  std::function<void(
402  const CVectorFixedDouble<6 + 3>& x, const double& dummy,
403  CVectorFixedDouble<3>& Y)>(&func_inv_compose_point),
404  x_incrs, DUMMY, numJacobs);
405 
406  num_df_dpose = numJacobs.block<3, 6>(0, 0);
407  num_df_dpoint = numJacobs.block<3, 3>(0, 6);
408  }
409 
410  EXPECT_NEAR(
411  0, (df_dpoint - num_df_dpoint).asEigen().array().abs().sum(), 3e-3)
412  << "p1: " << p1 << endl
413  << "p: " << p << endl
414  << "Numeric approximation of df_dpoint: " << endl
415  << num_df_dpoint << endl
416  << "Implemented method: " << endl
417  << df_dpoint << endl
418  << "Error: " << endl
419  << df_dpoint - num_df_dpoint << endl;
420 
421  EXPECT_NEAR(
422  0,
423  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
424  3e-3)
425  << "p1: " << p1 << endl
426  << "p: " << p << endl
427  << "Numeric approximation of df_dpose: " << endl
428  << num_df_dpose.asEigen() << endl
429  << "Implemented method: " << endl
430  << df_dpose.asEigen() << endl
431  << "Error: " << endl
432  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
433  }
434 
435  void test_default_values(const CPose3D& p, const std::string& label)
436  {
437  EXPECT_EQ(p.x(), 0);
438  EXPECT_EQ(p.y(), 0);
439  EXPECT_EQ(p.z(), 0);
440  EXPECT_EQ(p.yaw(), 0);
441  EXPECT_EQ(p.pitch(), 0);
442  EXPECT_EQ(p.roll(), 0);
443  for (size_t i = 0; i < 4; i++)
444  for (size_t j = 0; j < 4; j++)
445  EXPECT_NEAR(
446  p.getHomogeneousMatrixVal<CMatrixDouble44>()(i, j),
447  i == j ? 1.0 : 0.0, 1e-8)
448  << "Failed for (i,j)=" << i << "," << j << endl
449  << "Matrix is: " << endl
450  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
451  << "case was: " << label << endl;
452  }
453 
457  {
459  const CPoint3D p(P[0], P[1], P[2]);
460  const CPoint3D pp = q + p;
461  for (int i = 0; i < 3; i++) Y[i] = pp[i];
462  }
463 
467  {
469  const CPoint3D p(P[0], P[1], P[2]);
470  const CPoint3D pp = p - q;
471  for (int i = 0; i < 3; i++) Y[i] = pp[i];
472  }
473 
475  {
477 
478  TPoint3D pp;
479  p.composePoint(
480  x_l.x, x_l.y, x_l.z, pp.x, pp.y, pp.z, std::nullopt, std::nullopt,
481  df_dse3);
482 
483  // Numerical approx:
485  {
486  CVectorFixedDouble<6> x_mean;
487  for (int i = 0; i < 6; i++) x_mean[i] = 0;
488 
490  for (int i = 0; i < 3; i++) P[i] = pp[i];
491 
492  CVectorFixedDouble<6> x_incrs;
493  x_incrs.fill(1e-9);
495  x_mean,
496  std::function<void(
497  const CVectorFixedDouble<6>& x,
499  &func_compose_point_se3),
500  x_incrs, P, num_df_dse3);
501  }
502 
503  EXPECT_NEAR(
504  0, (df_dse3.asEigen() - num_df_dse3.asEigen()).array().abs().sum(),
505  3e-3)
506  << "p: " << p << endl
507  << "x_l: " << x_l << endl
508  << "Numeric approximation of df_dse3: " << endl
509  << num_df_dse3.asEigen() << endl
510  << "Implemented method: " << endl
511  << df_dse3.asEigen() << endl
512  << "Error: " << endl
513  << df_dse3.asEigen() - num_df_dse3.asEigen() << endl;
514  }
515 
517  {
519 
520  TPoint3D pp;
521  p.inverseComposePoint(
522  x_g.x, x_g.y, x_g.z, pp.x, pp.y, pp.z, std::nullopt, std::nullopt,
523  df_dse3);
524 
525  // Numerical approx:
527  {
528  CVectorFixedDouble<6> x_mean;
529  for (int i = 0; i < 6; i++) x_mean[i] = 0;
530 
532  for (int i = 0; i < 3; i++) P[i] = pp[i];
533 
534  CVectorFixedDouble<6> x_incrs;
535  x_incrs.fill(1e-9);
537  x_mean,
538  std::function<void(
539  const CVectorFixedDouble<6>& x,
541  &func_invcompose_point_se3),
542  x_incrs, P, num_df_dse3);
543  }
544 
545  EXPECT_NEAR(
546  0, (df_dse3.asEigen() - num_df_dse3.asEigen()).array().abs().sum(),
547  3e-3)
548  << "p: " << p << endl
549  << "x_g: " << x_g << endl
550  << "Numeric approximation of df_dse3: " << endl
551  << num_df_dse3.asEigen() << endl
552  << "Implemented method: " << endl
553  << df_dse3.asEigen() << endl
554  << "Error: " << endl
555  << df_dse3.asEigen() - num_df_dse3.asEigen() << endl;
556  }
557 
558  static void func_jacob_expe_e(
559  const CVectorFixedDouble<6>& x, const double& dummy,
561  {
562  MRPT_UNUSED_PARAM(dummy);
563  const CPose3D p = Lie::SE<3>::exp(x);
564  // const CMatrixDouble44 R =
565  // p.getHomogeneousMatrixVal<CMatrixDouble44>();
566  p.getAs12Vector(Y);
567  }
568 
569  // Check dexp(e)_de
571  {
572  CVectorFixedDouble<6> x_mean;
573  for (int i = 0; i < 6; i++) x_mean[i] = 0;
574 
575  double dummy = 0.;
576  CVectorFixedDouble<6> x_incrs;
577  x_incrs.fill(1e-9);
578  CMatrixDouble numJacobs;
580  x_mean,
581  std::function<void(
582  const CVectorFixedDouble<6>& x, const double& dummy,
583  CVectorFixedDouble<12>& Y)>(&func_jacob_expe_e),
584  x_incrs, dummy, numJacobs);
585 
586  // Theoretical matrix:
587  // [ 0 -[e1]_x ]
588  // [ 0 -[e2]_x ]
589  // [ 0 -[e3]_x ]
590  // [ I_3 0 ]
591  double vals[12 * 6] = {
592  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0,
593 
594  0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
595 
596  0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0,
597 
598  1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0};
600 
601  EXPECT_NEAR(
602  (numJacobs.asEigen() - M.asEigen()).array().abs().maxCoeff(), 0,
603  1e-5)
604  << "M:\n"
605  << M.asEigen() << "numJacobs:\n"
606  << numJacobs << "\n";
607  }
608 
609  static void func_jacob_LnT_T(
610  const CVectorFixedDouble<12>& x, const double& dummy,
612  {
613  MRPT_UNUSED_PARAM(dummy);
614  CPose3D p;
615 
616  p.setFrom12Vector(x);
617  // ensure 3x3 rot vector is orthonormal (Sophus complains otherwise):
618  auto R = p.getRotationMatrix();
619  const auto Rsvd =
620  R.asEigen().jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
621  R = Rsvd.matrixU() * Rsvd.matrixV().transpose();
622  p.setRotationMatrix(R);
623 
624  Y = Lie::SE<3>::log(p);
625  }
626 
627  // Jacobian of Ln(T) wrt T
629  {
630  const auto theor_jacob = Lie::SE<3>::jacob_dlogv_dv(p);
631 
632  CMatrixDouble numJacobs;
633  {
634  CVectorFixedDouble<12> x_mean;
635  p.getAs12Vector(x_mean);
636 
637  double dummy = 0.;
638  CVectorFixedDouble<12> x_incrs;
639  x_incrs.fill(1e-6);
641  x_mean,
642  std::function<void(
643  const CVectorFixedDouble<12>& x, const double& dummy,
644  CVectorFixedDouble<6>& Y)>(&func_jacob_LnT_T),
645  x_incrs, dummy, numJacobs);
646  }
647 
648  EXPECT_NEAR(
649  (numJacobs.asEigen() - theor_jacob.asEigen()).array().abs().sum(),
650  0, 1e-3)
651  << "Pose: " << p << endl
652  << "Pose matrix:\n"
653  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
654  << numJacobs << endl
655  << "Theor. Jacob:\n"
656  << theor_jacob.asEigen() << endl
657  << "ERR:\n"
658  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
659  }
660 
661  static void func_jacob_expe_D(
662  const CVectorFixedDouble<6>& eps, const CPose3D& D,
664  {
665  const CPose3D incr = Lie::SE<3>::exp(eps);
666  const CPose3D expe_D = incr + D;
667  expe_D.getAs12Vector(Y);
668  }
669 
670  // Test Jacobian: d exp(e)*D / d e
671  // 10.3.3 in tech report
673  {
674  const auto theor_jacob = Lie::SE<3>::jacob_dexpeD_de(p);
675 
676  CMatrixDouble numJacobs;
677  {
678  CVectorFixedDouble<6> x_mean;
679  x_mean.setZero();
680 
681  CVectorFixedDouble<6> x_incrs;
682  x_incrs.fill(1e-6);
684  x_mean,
685  std::function<void(
686  const CVectorFixedDouble<6>& eps, const CPose3D& D,
687  CVectorFixedDouble<12>& Y)>(&func_jacob_expe_D),
688  x_incrs, p, numJacobs);
689  }
690 
691  EXPECT_NEAR(
692  (numJacobs.asEigen() - theor_jacob.asEigen())
693  .array()
694  .abs()
695  .maxCoeff(),
696  0, 1e-3)
697  << "Pose: " << p << endl
698  << "Pose matrix:\n"
699  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
700  << numJacobs << endl
701  << "Theor. Jacob:\n"
702  << theor_jacob.asEigen() << endl
703  << "ERR:\n"
704  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
705  }
706 
707  static void func_jacob_D_expe(
708  const CVectorFixedDouble<6>& eps, const CPose3D& D,
710  {
711  const CPose3D incr = Lie::SE<3>::exp(eps);
712  const CPose3D expe_D = D + incr;
713  expe_D.getAs12Vector(Y);
714  }
715 
716  // Test Jacobian: d D*exp(e) / d e
717  // 10.3.4 in tech report
719  {
720  const auto theor_jacob = Lie::SE<3>::jacob_dDexpe_de(p);
721 
722  CMatrixDouble numJacobs;
723  {
724  CVectorFixedDouble<6> x_mean;
725  x_mean.setZero();
726 
727  CVectorFixedDouble<6> x_incrs;
728  x_incrs.fill(1e-6);
730  x_mean,
731  std::function<void(
732  const CVectorFixedDouble<6>& eps, const CPose3D& D,
733  CVectorFixedDouble<12>& Y)>(&func_jacob_D_expe),
734  x_incrs, p, numJacobs);
735  }
736 
737  EXPECT_NEAR(
738  (numJacobs.asEigen() - theor_jacob.asEigen())
739  .array()
740  .abs()
741  .maxCoeff(),
742  0, 1e-3)
743  << "Pose: " << p << endl
744  << "Pose matrix:\n"
745  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
746  << numJacobs << endl
747  << "Theor. Jacob:\n"
748  << theor_jacob.asEigen() << endl
749  << "ERR:\n"
750  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
751  }
752 
754  {
756  };
757 
758  static void func_jacob_Aexpe_D(
759  const CVectorFixedDouble<6>& eps,
761  {
762  const CPose3D incr = Lie::SE<3>::exp(eps);
763  const CPose3D res = params.A + incr + params.D;
764  res.getAs12Vector(Y);
765  }
766 
767  // Test Jacobian: d A*exp(e)*D / d e
768  // 10.3.7 in tech report
769  // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
770  void test_Jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D)
771  {
772  const auto theor_jacob = Lie::SE<3>::jacob_dAexpeD_de(A, D);
773 
774  CMatrixDouble numJacobs;
775  {
776  CVectorFixedDouble<6> x_mean;
777  x_mean.setZero();
778 
780  params.A = A;
781  params.D = D;
782  CVectorFixedDouble<6> x_incrs;
783  x_incrs.fill(1e-6);
785  x_mean,
786  std::function<void(
787  const CVectorFixedDouble<6>& eps,
789  CVectorFixedDouble<12>& Y)>(&func_jacob_Aexpe_D),
790  x_incrs, params, numJacobs);
791  }
792 
793  EXPECT_NEAR(
794  (numJacobs.asEigen() - theor_jacob.asEigen())
795  .array()
796  .abs()
797  .maxCoeff(),
798  0, 1e-3)
799  << "Pose A: " << A << endl
800  << "Pose D: " << D << endl
801  << "Num. Jacob:\n"
802  << numJacobs << endl
803  << "Theor. Jacob:\n"
804  << theor_jacob.asEigen() << endl
805  << "ERR:\n"
806  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
807  }
808 };
809 
810 // Elemental tests:
811 TEST_F(Pose3DTests, DefaultValues)
812 {
813  {
814  CPose3D p;
815  test_default_values(p, "Default");
816  }
817  {
818  CPose3D p2;
819  CPose3D p = p2;
820  test_default_values(p, "p=p2");
821  }
822  {
823  CPose3D p1, p2;
824  test_default_values(p1 + p2, "p1+p2");
825  CPose3D p = p1 + p2;
826  test_default_values(p, "p=p1+p2");
827  }
828  {
829  CPose3D p1, p2;
830  CPose3D p = p1 - p2;
831  test_default_values(p, "p1-p2");
832  }
833 }
834 
835 TEST_F(Pose3DTests, Initialization)
836 {
837  CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
838  EXPECT_NEAR(p.x(), 1, 1e-7);
839  EXPECT_NEAR(p.y(), 2, 1e-7);
840  EXPECT_NEAR(p.z(), 3, 1e-7);
841  EXPECT_NEAR(p.yaw(), 0.2, 1e-7);
842  EXPECT_NEAR(p.pitch(), 0.3, 1e-7);
843  EXPECT_NEAR(p.roll(), 0.4, 1e-7);
844 }
845 
846 TEST_F(Pose3DTests, OperatorBracket)
847 {
848  CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
849  EXPECT_NEAR(p[0], 1, 1e-7);
850  EXPECT_NEAR(p[1], 2, 1e-7);
851  EXPECT_NEAR(p[2], 3, 1e-7);
852  EXPECT_NEAR(p[3], 0.2, 1e-7);
853  EXPECT_NEAR(p[4], 0.3, 1e-7);
854  EXPECT_NEAR(p[5], 0.4, 1e-7);
855 }
856 
857 // List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in
858 // degrees)
859 static const std::vector<mrpt::poses::CPose3D> ptc = {
860  {.0, .0, .0, .0_deg, .0_deg, .0_deg},
861  {1.0, 2.0, 3.0, .0_deg, .0_deg, .0_deg},
862  {1.0, 2.0, 3.0, 10.0_deg, .0_deg, .0_deg},
863  {1.0, 2.0, 3.0, .0_deg, 1.0_deg, .0_deg},
864  {1.0, 2.0, 3.0, .0_deg, .0_deg, 1.0_deg},
865  {1.0, 2.0, 3.0, 80.0_deg, 5.0_deg, 5.0_deg},
866  {1.0, 2.0, 3.0, -20.0_deg, -30.0_deg, -40.0_deg},
867  {1.0, 2.0, 3.0, -45.0_deg, 10.0_deg, 70.0_deg},
868  {1.0, 2.0, 3.0, 40.0_deg, -5.0_deg, 25.0_deg},
869  {1.0, 2.0, 3.0, 40.0_deg, 20.0_deg, -15.0_deg},
870  {-6.0, 2.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
871  {6.0, -5.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
872  {6.0, 2.0, -9.0, 40.0_deg, 20.0_deg, 15.0_deg},
873  {0.0, 8.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
874  {1.0, 0.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
875  {1.0, 8.0, 0.0, -45.0_deg, 10.0_deg, 70.0_deg}};
876 
877 // More complex tests:
878 TEST_F(Pose3DTests, InverseHM)
879 {
880  for (const auto& p : ptc) test_inverse(p);
881 }
882 
884 {
885  for (const auto& p1 : ptc)
886  for (const auto& p2 : ptc) test_compose(p1, p2);
887 }
888 TEST_F(Pose3DTests, composeFrom)
889 {
890  for (const auto& p1 : ptc)
891  for (const auto& p2 : ptc) test_composeFrom(p1, p2);
892 }
893 
894 TEST_F(Pose3DTests, ToFromCPose2D)
895 {
896  for (const auto& p : ptc) test_to_from_2d(p.x(), p.y(), p.yaw());
897 }
898 
899 TEST_F(Pose3DTests, ComposeAndInvComposeWithPoint)
900 {
901  for (const auto& p : ptc)
902  {
903  test_composePoint(p, 10, 11, 12);
904  test_composePoint(p, -5, 1, 2);
905  test_composePoint(p, 5, -1, 2);
906  test_composePoint(p, 5, 1, -2);
907  }
908 }
909 
910 TEST_F(Pose3DTests, ComposePointJacob)
911 {
912  for (const auto& p : ptc)
913  {
914  test_composePointJacob(p, 10, 11, 12);
915  test_composePointJacob(p, -5, 1, 2);
916  }
917 }
918 
919 TEST_F(Pose3DTests, ComposePointJacobApprox)
920 { // Test approximated Jacobians for very small rotations
921  test_composePointJacob(
922  mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg), 10, 11,
923  12, true);
924  test_composePointJacob(
925  mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.1_deg, 0.0_deg, 0.0_deg), 10, 11,
926  12, true);
927  test_composePointJacob(
928  mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.1_deg, 0.0_deg), 10, 11,
929  12, true);
930  test_composePointJacob(
931  mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.1_deg), 10, 11,
932  12, true);
933 }
934 
935 TEST_F(Pose3DTests, InvComposePointJacob)
936 {
937  for (const auto& p : ptc)
938  {
939  test_invComposePointJacob(p, 10, 11, 12);
940  test_invComposePointJacob(p, -5, 1, 2);
941  test_invComposePointJacob(p, 5, -1, 2);
942  test_invComposePointJacob(p, 5, 1, -2);
943  }
944 }
945 
946 TEST_F(Pose3DTests, ComposePointJacob_se3)
947 {
948  for (const auto& p : ptc)
949  {
950  test_composePointJacob_se3(p, TPoint3D(0, 0, 0));
951  test_composePointJacob_se3(p, TPoint3D(10, 11, 12));
952  test_composePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
953  }
954 }
955 TEST_F(Pose3DTests, InvComposePointJacob_se3)
956 {
957  for (const auto& p : ptc)
958  {
959  test_invComposePointJacob_se3(p, TPoint3D(0, 0, 0));
960  test_invComposePointJacob_se3(p, TPoint3D(10, 11, 12));
961  test_invComposePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
962  }
963 }
964 
965 TEST_F(Pose3DTests, ExpLnEqual)
966 {
967  for (const auto& p : ptc) test_ExpLnEqual(p);
968 }
969 
970 TEST_F(Pose3DTests, Jacob_dExpe_de_at_0) { check_jacob_expe_e_at_0(); }
971 TEST_F(Pose3DTests, Jacob_dLnT_dT)
972 {
973  check_jacob_LnT_T(mrpt::poses::CPose3D(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg));
974  // JL NOTE:
975  // This function cannot be properly tested numerically, since the logm()
976  // implementation
977  // is not generic and does NOT depends on all matrix entries, thus the
978  // numerical Jacobian
979  // contains entire columns of zeros, even if the theorethical doesn't.
980  // check_jacob_LnT_T(1.0,0,0, 0.0_deg,0.0_deg,0.0_deg ); ...
981 }
982 
983 TEST_F(Pose3DTests, Jacob_dexpeD_de)
984 {
985  for (const auto& p : ptc) test_Jacob_dexpeD_de(p);
986 }
987 
988 TEST_F(Pose3DTests, Jacob_dDexpe_de)
989 {
990  for (const auto& p : ptc) test_Jacob_dDexpe_de(p);
991 }
992 
993 TEST_F(Pose3DTests, Jacob_dAexpeD_de)
994 {
995  for (const auto& p1 : ptc)
996  for (const auto& p2 : ptc) test_Jacob_dAexpeD_de(p1, p2);
997 }
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)
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
void test_invComposePointJacob_se3(const CPose3D &p, const TPoint3D x_g)
static const std::vector< mrpt::poses::CPose3D > ptc
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
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)
static void func_inv_compose_point(const CVectorFixedDouble< 6+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
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
STL namespace.
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:600
void test_to_from_2d(double x, double y, double phi)
static void func_jacob_Aexpe_D(const CVectorFixedDouble< 6 > &eps, const TParams_func_jacob_Aexpe_D &params, CVectorFixedDouble< 12 > &Y)
void check_jacob_expe_e_at_0()
static void func_jacob_LnT_T(const CVectorFixedDouble< 12 > &x, const double &dummy, CVectorFixedDouble< 6 > &Y)
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:572
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)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
GLsizei const GLchar ** string
Definition: glext.h:4116
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...
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)
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
void test_Jacob_dDexpe_de(const CPose3D &p)
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
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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
static void func_compose_point(const CVectorFixedDouble< 6+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
GLenum GLint GLint y
Definition: glext.h:3542
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
GLuint res
Definition: glext.h:7385
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
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
GLfloat GLfloat p
Definition: glext.h:6398
GLenum const GLfloat * params
Definition: glext.h:3538
static void func_jacob_expe_e(const CVectorFixedDouble< 6 > &x, const double &dummy, CVectorFixedDouble< 12 > &Y)
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
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void SetUp() override



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 1d7b2c899 Thu Nov 14 18:59:07 2019 +0100 at jue nov 14 19:15:09 CET 2019