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 
39  EXPECT_NEAR(
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 
55  EXPECT_NEAR(
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 
78  EXPECT_NEAR(
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 
89  EXPECT_NEAR(
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
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, nullptr,
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;
384  x, y, z, pp.x, pp.y, pp.z, &df_dpoint, &df_dpose);
385 
386  // Numerical approx:
389  {
391  for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
392  x_mean[6 + 0] = x;
393  x_mean[6 + 1] = y;
394  x_mean[6 + 2] = z;
395 
396  double DUMMY = 0;
398  x_incrs.fill(1e-7);
399  CMatrixDouble numJacobs;
401  x_mean,
402  std::function<void(
403  const CVectorFixedDouble<6 + 3>& x, const double& dummy,
404  CVectorFixedDouble<3>& Y)>(&func_inv_compose_point),
405  x_incrs, DUMMY, numJacobs);
406 
407  num_df_dpose = numJacobs.block<3, 6>(0, 0);
408  num_df_dpoint = numJacobs.block<3, 3>(0, 6);
409  }
410 
411  EXPECT_NEAR(
412  0, (df_dpoint - num_df_dpoint).asEigen().array().abs().sum(), 3e-3)
413  << "p1: " << p1 << endl
414  << "p: " << p << endl
415  << "Numeric approximation of df_dpoint: " << endl
416  << num_df_dpoint << endl
417  << "Implemented method: " << endl
418  << df_dpoint << endl
419  << "Error: " << endl
420  << df_dpoint - num_df_dpoint << endl;
421 
422  EXPECT_NEAR(
423  0,
424  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
425  3e-3)
426  << "p1: " << p1 << endl
427  << "p: " << p << endl
428  << "Numeric approximation of df_dpose: " << endl
429  << num_df_dpose.asEigen() << endl
430  << "Implemented method: " << endl
431  << df_dpose.asEigen() << endl
432  << "Error: " << endl
433  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
434  }
435 
436  void test_default_values(const CPose3D& p, const std::string& label)
437  {
438  EXPECT_EQ(p.x(), 0);
439  EXPECT_EQ(p.y(), 0);
440  EXPECT_EQ(p.z(), 0);
441  EXPECT_EQ(p.yaw(), 0);
442  EXPECT_EQ(p.pitch(), 0);
443  EXPECT_EQ(p.roll(), 0);
444  for (size_t i = 0; i < 4; i++)
445  for (size_t j = 0; j < 4; j++)
446  EXPECT_NEAR(
447  p.getHomogeneousMatrixVal<CMatrixDouble44>()(i, j),
448  i == j ? 1.0 : 0.0, 1e-8)
449  << "Failed for (i,j)=" << i << "," << j << endl
450  << "Matrix is: " << endl
451  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
452  << "case was: " << label << endl;
453  }
454 
458  {
460  const CPoint3D p(P[0], P[1], P[2]);
461  const CPoint3D pp = q + p;
462  for (int i = 0; i < 3; i++) Y[i] = pp[i];
463  }
464 
468  {
470  const CPoint3D p(P[0], P[1], P[2]);
471  const CPoint3D pp = p - q;
472  for (int i = 0; i < 3; i++) Y[i] = pp[i];
473  }
474 
476  {
478 
479  TPoint3D pp;
480  p.composePoint(
481  x_l.x, x_l.y, x_l.z, pp.x, pp.y, pp.z, nullptr, nullptr, &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, nullptr, nullptr, &df_dse3);
523 
524  // Numerical approx:
526  {
527  CVectorFixedDouble<6> x_mean;
528  for (int i = 0; i < 6; i++) x_mean[i] = 0;
529 
531  for (int i = 0; i < 3; i++) P[i] = pp[i];
532 
533  CVectorFixedDouble<6> x_incrs;
534  x_incrs.fill(1e-9);
536  x_mean,
537  std::function<void(
538  const CVectorFixedDouble<6>& x,
540  &func_invcompose_point_se3),
541  x_incrs, P, num_df_dse3);
542  }
543 
544  EXPECT_NEAR(
545  0, (df_dse3.asEigen() - num_df_dse3.asEigen()).array().abs().sum(),
546  3e-3)
547  << "p: " << p << endl
548  << "x_g: " << x_g << endl
549  << "Numeric approximation of df_dse3: " << endl
550  << num_df_dse3.asEigen() << endl
551  << "Implemented method: " << endl
552  << df_dse3.asEigen() << endl
553  << "Error: " << endl
554  << df_dse3.asEigen() - num_df_dse3.asEigen() << endl;
555  }
556 
557  static void func_jacob_expe_e(
558  const CVectorFixedDouble<6>& x, const double& dummy,
560  {
561  MRPT_UNUSED_PARAM(dummy);
562  const CPose3D p = Lie::SE<3>::exp(x);
563  // const CMatrixDouble44 R =
564  // p.getHomogeneousMatrixVal<CMatrixDouble44>();
565  p.getAs12Vector(Y);
566  }
567 
568  // Check dexp(e)_de
570  {
571  CVectorFixedDouble<6> x_mean;
572  for (int i = 0; i < 6; i++) x_mean[i] = 0;
573 
574  double dummy = 0.;
575  CVectorFixedDouble<6> x_incrs;
576  x_incrs.fill(1e-9);
577  CMatrixDouble numJacobs;
579  x_mean,
580  std::function<void(
581  const CVectorFixedDouble<6>& x, const double& dummy,
582  CVectorFixedDouble<12>& Y)>(&func_jacob_expe_e),
583  x_incrs, dummy, numJacobs);
584 
585  // Theoretical matrix:
586  // [ 0 -[e1]_x ]
587  // [ 0 -[e2]_x ]
588  // [ 0 -[e3]_x ]
589  // [ I_3 0 ]
590  double vals[12 * 6] = {
591  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0,
592 
593  0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
594 
595  0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0,
596 
597  1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0};
599 
600  EXPECT_NEAR(
601  (numJacobs.asEigen() - M.asEigen()).array().abs().maxCoeff(), 0,
602  1e-5)
603  << "M:\n"
604  << M.asEigen() << "numJacobs:\n"
605  << numJacobs << "\n";
606  }
607 
608  static void func_jacob_LnT_T(
609  const CVectorFixedDouble<12>& x, const double& dummy,
611  {
612  MRPT_UNUSED_PARAM(dummy);
613  CPose3D p;
614 
615  p.setFrom12Vector(x);
616  // ensure 3x3 rot vector is orthonormal (Sophus complains otherwise):
617  auto R = p.getRotationMatrix();
618  const auto Rsvd =
619  R.asEigen().jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
620  R = Rsvd.matrixU() * Rsvd.matrixV().transpose();
621  p.setRotationMatrix(R);
622 
623  Y = Lie::SE<3>::log(p);
624  }
625 
626  // Jacobian of Ln(T) wrt T
628  {
629  const auto theor_jacob = Lie::SE<3>::jacob_dlogv_dv(p);
630 
631  CMatrixDouble numJacobs;
632  {
633  CVectorFixedDouble<12> x_mean;
634  p.getAs12Vector(x_mean);
635 
636  double dummy = 0.;
637  CVectorFixedDouble<12> x_incrs;
638  x_incrs.fill(1e-6);
640  x_mean,
641  std::function<void(
642  const CVectorFixedDouble<12>& x, const double& dummy,
643  CVectorFixedDouble<6>& Y)>(&func_jacob_LnT_T),
644  x_incrs, dummy, numJacobs);
645  }
646 
647  EXPECT_NEAR(
648  (numJacobs.asEigen() - theor_jacob.asEigen()).array().abs().sum(),
649  0, 1e-3)
650  << "Pose: " << p << endl
651  << "Pose matrix:\n"
652  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
653  << numJacobs << endl
654  << "Theor. Jacob:\n"
655  << theor_jacob.asEigen() << endl
656  << "ERR:\n"
657  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
658  }
659 
660  static void func_jacob_expe_D(
661  const CVectorFixedDouble<6>& eps, const CPose3D& D,
663  {
664  const CPose3D incr = Lie::SE<3>::exp(eps);
665  const CPose3D expe_D = incr + D;
666  expe_D.getAs12Vector(Y);
667  }
668 
669  // Test Jacobian: d exp(e)*D / d e
670  // 10.3.3 in tech report
672  {
673  const auto theor_jacob = Lie::SE<3>::jacob_dexpeD_de(p);
674 
675  CMatrixDouble numJacobs;
676  {
677  CVectorFixedDouble<6> x_mean;
678  x_mean.setZero();
679 
680  CVectorFixedDouble<6> x_incrs;
681  x_incrs.fill(1e-6);
683  x_mean,
684  std::function<void(
685  const CVectorFixedDouble<6>& eps, const CPose3D& D,
686  CVectorFixedDouble<12>& Y)>(&func_jacob_expe_D),
687  x_incrs, p, numJacobs);
688  }
689 
690  EXPECT_NEAR(
691  (numJacobs.asEigen() - theor_jacob.asEigen())
692  .array()
693  .abs()
694  .maxCoeff(),
695  0, 1e-3)
696  << "Pose: " << p << endl
697  << "Pose matrix:\n"
698  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
699  << numJacobs << endl
700  << "Theor. Jacob:\n"
701  << theor_jacob.asEigen() << endl
702  << "ERR:\n"
703  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
704  }
705 
706  static void func_jacob_D_expe(
707  const CVectorFixedDouble<6>& eps, const CPose3D& D,
709  {
710  const CPose3D incr = Lie::SE<3>::exp(eps);
711  const CPose3D expe_D = D + incr;
712  expe_D.getAs12Vector(Y);
713  }
714 
715  // Test Jacobian: d D*exp(e) / d e
716  // 10.3.4 in tech report
718  {
719  const auto theor_jacob = Lie::SE<3>::jacob_dDexpe_de(p);
720 
721  CMatrixDouble numJacobs;
722  {
723  CVectorFixedDouble<6> x_mean;
724  x_mean.setZero();
725 
726  CVectorFixedDouble<6> x_incrs;
727  x_incrs.fill(1e-6);
729  x_mean,
730  std::function<void(
731  const CVectorFixedDouble<6>& eps, const CPose3D& D,
732  CVectorFixedDouble<12>& Y)>(&func_jacob_D_expe),
733  x_incrs, p, numJacobs);
734  }
735 
736  EXPECT_NEAR(
737  (numJacobs.asEigen() - theor_jacob.asEigen())
738  .array()
739  .abs()
740  .maxCoeff(),
741  0, 1e-3)
742  << "Pose: " << p << endl
743  << "Pose matrix:\n"
744  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
745  << numJacobs << endl
746  << "Theor. Jacob:\n"
747  << theor_jacob.asEigen() << endl
748  << "ERR:\n"
749  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
750  }
751 
753  {
755  };
756 
757  static void func_jacob_Aexpe_D(
758  const CVectorFixedDouble<6>& eps,
760  {
761  const CPose3D incr = Lie::SE<3>::exp(eps);
762  const CPose3D res = params.A + incr + params.D;
763  res.getAs12Vector(Y);
764  }
765 
766  // Test Jacobian: d A*exp(e)*D / d e
767  // 10.3.7 in tech report
768  // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
769  void test_Jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D)
770  {
771  const auto theor_jacob = Lie::SE<3>::jacob_dAexpeD_de(A, D);
772 
773  CMatrixDouble numJacobs;
774  {
775  CVectorFixedDouble<6> x_mean;
776  x_mean.setZero();
777 
779  params.A = A;
780  params.D = D;
781  CVectorFixedDouble<6> x_incrs;
782  x_incrs.fill(1e-6);
784  x_mean,
785  std::function<void(
786  const CVectorFixedDouble<6>& eps,
788  CVectorFixedDouble<12>& Y)>(&func_jacob_Aexpe_D),
789  x_incrs, params, numJacobs);
790  }
791 
792  EXPECT_NEAR(
793  (numJacobs.asEigen() - theor_jacob.asEigen())
794  .array()
795  .abs()
796  .maxCoeff(),
797  0, 1e-3)
798  << "Pose A: " << A << endl
799  << "Pose D: " << D << endl
800  << "Num. Jacob:\n"
801  << numJacobs << endl
802  << "Theor. Jacob:\n"
803  << theor_jacob.asEigen() << endl
804  << "ERR:\n"
805  << theor_jacob.asEigen() - numJacobs.asEigen() << endl;
806  }
807 };
808 
809 // Elemental tests:
810 TEST_F(Pose3DTests, DefaultValues)
811 {
812  {
813  CPose3D p;
814  test_default_values(p, "Default");
815  }
816  {
817  CPose3D p2;
818  CPose3D p = p2;
819  test_default_values(p, "p=p2");
820  }
821  {
822  CPose3D p1, p2;
823  test_default_values(p1 + p2, "p1+p2");
824  CPose3D p = p1 + p2;
825  test_default_values(p, "p=p1+p2");
826  }
827  {
828  CPose3D p1, p2;
829  CPose3D p = p1 - p2;
830  test_default_values(p, "p1-p2");
831  }
832 }
833 
834 TEST_F(Pose3DTests, Initialization)
835 {
836  CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
837  EXPECT_NEAR(p.x(), 1, 1e-7);
838  EXPECT_NEAR(p.y(), 2, 1e-7);
839  EXPECT_NEAR(p.z(), 3, 1e-7);
840  EXPECT_NEAR(p.yaw(), 0.2, 1e-7);
841  EXPECT_NEAR(p.pitch(), 0.3, 1e-7);
842  EXPECT_NEAR(p.roll(), 0.4, 1e-7);
843 }
844 
845 TEST_F(Pose3DTests, OperatorBracket)
846 {
847  CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
848  EXPECT_NEAR(p[0], 1, 1e-7);
849  EXPECT_NEAR(p[1], 2, 1e-7);
850  EXPECT_NEAR(p[2], 3, 1e-7);
851  EXPECT_NEAR(p[3], 0.2, 1e-7);
852  EXPECT_NEAR(p[4], 0.3, 1e-7);
853  EXPECT_NEAR(p[5], 0.4, 1e-7);
854 }
855 
856 // List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in
857 // degrees)
858 static const std::vector<mrpt::poses::CPose3D> ptc = {
859  {.0, .0, .0, DEG2RAD(.0), DEG2RAD(.0), DEG2RAD(.0)},
860  {1.0, 2.0, 3.0, DEG2RAD(.0), DEG2RAD(.0), DEG2RAD(.0)},
861  {1.0, 2.0, 3.0, DEG2RAD(10.0), DEG2RAD(.0), DEG2RAD(.0)},
862  {1.0, 2.0, 3.0, DEG2RAD(.0), DEG2RAD(1.0), DEG2RAD(.0)},
863  {1.0, 2.0, 3.0, DEG2RAD(.0), DEG2RAD(.0), DEG2RAD(1.0)},
864  {1.0, 2.0, 3.0, DEG2RAD(80.0), DEG2RAD(5.0), DEG2RAD(5.0)},
865  {1.0, 2.0, 3.0, DEG2RAD(-20.0), DEG2RAD(-30.0), DEG2RAD(-40.0)},
866  {1.0, 2.0, 3.0, DEG2RAD(-45.0), DEG2RAD(10.0), DEG2RAD(70.0)},
867  {1.0, 2.0, 3.0, DEG2RAD(40.0), DEG2RAD(-5.0), DEG2RAD(25.0)},
868  {1.0, 2.0, 3.0, DEG2RAD(40.0), DEG2RAD(20.0), DEG2RAD(-15.0)},
869  {-6.0, 2.0, 3.0, DEG2RAD(40.0), DEG2RAD(20.0), DEG2RAD(15.0)},
870  {6.0, -5.0, 3.0, DEG2RAD(40.0), DEG2RAD(20.0), DEG2RAD(15.0)},
871  {6.0, 2.0, -9.0, DEG2RAD(40.0), DEG2RAD(20.0), DEG2RAD(15.0)},
872  {0.0, 8.0, 5.0, DEG2RAD(-45.0), DEG2RAD(10.0), DEG2RAD(70.0)},
873  {1.0, 0.0, 5.0, DEG2RAD(-45.0), DEG2RAD(10.0), DEG2RAD(70.0)},
874  {1.0, 8.0, 0.0, DEG2RAD(-45.0), DEG2RAD(10.0), DEG2RAD(70.0)}};
875 
876 // More complex tests:
877 TEST_F(Pose3DTests, InverseHM)
878 {
879  for (const auto& p : ptc) test_inverse(p);
880 }
881 
883 {
884  for (const auto& p1 : ptc)
885  for (const auto& p2 : ptc) test_compose(p1, p2);
886 }
887 TEST_F(Pose3DTests, composeFrom)
888 {
889  for (const auto& p1 : ptc)
890  for (const auto& p2 : ptc) test_composeFrom(p1, p2);
891 }
892 
893 TEST_F(Pose3DTests, ToFromCPose2D)
894 {
895  for (const auto& p : ptc) test_to_from_2d(p.x(), p.y(), p.yaw());
896 }
897 
898 TEST_F(Pose3DTests, ComposeAndInvComposeWithPoint)
899 {
900  for (const auto& p : ptc)
901  {
902  test_composePoint(p, 10, 11, 12);
903  test_composePoint(p, -5, 1, 2);
904  test_composePoint(p, 5, -1, 2);
905  test_composePoint(p, 5, 1, -2);
906  }
907 }
908 
909 TEST_F(Pose3DTests, ComposePointJacob)
910 {
911  for (const auto& p : ptc)
912  {
913  test_composePointJacob(p, 10, 11, 12);
914  test_composePointJacob(p, -5, 1, 2);
915  }
916 }
917 
918 TEST_F(Pose3DTests, ComposePointJacobApprox)
919 { // Test approximated Jacobians for very small rotations
920  test_composePointJacob(
921  mrpt::poses::CPose3D(1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0)),
922  10, 11, 12, true);
923  test_composePointJacob(
925  1.0, 2.0, 3.0, DEG2RAD(0.1), DEG2RAD(0), DEG2RAD(0)),
926  10, 11, 12, true);
927  test_composePointJacob(
929  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0.1), DEG2RAD(0)),
930  10, 11, 12, true);
931  test_composePointJacob(
933  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0.1)),
934  10, 11, 12, true);
935 }
936 
937 TEST_F(Pose3DTests, InvComposePointJacob)
938 {
939  for (const auto& p : ptc)
940  {
941  test_invComposePointJacob(p, 10, 11, 12);
942  test_invComposePointJacob(p, -5, 1, 2);
943  test_invComposePointJacob(p, 5, -1, 2);
944  test_invComposePointJacob(p, 5, 1, -2);
945  }
946 }
947 
948 TEST_F(Pose3DTests, ComposePointJacob_se3)
949 {
950  for (const auto& p : ptc)
951  {
952  test_composePointJacob_se3(p, TPoint3D(0, 0, 0));
953  test_composePointJacob_se3(p, TPoint3D(10, 11, 12));
954  test_composePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
955  }
956 }
957 TEST_F(Pose3DTests, InvComposePointJacob_se3)
958 {
959  for (const auto& p : ptc)
960  {
961  test_invComposePointJacob_se3(p, TPoint3D(0, 0, 0));
962  test_invComposePointJacob_se3(p, TPoint3D(10, 11, 12));
963  test_invComposePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
964  }
965 }
966 
967 TEST_F(Pose3DTests, ExpLnEqual)
968 {
969  for (const auto& p : ptc) test_ExpLnEqual(p);
970 }
971 
972 TEST_F(Pose3DTests, Jacob_dExpe_de_at_0) { check_jacob_expe_e_at_0(); }
973 TEST_F(Pose3DTests, Jacob_dLnT_dT)
974 {
975  check_jacob_LnT_T(
976  mrpt::poses::CPose3D(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0)));
977  // JL NOTE:
978  // This function cannot be properly tested numerically, since the logm()
979  // implementation
980  // is not generic and does NOT depends on all matrix entries, thus the
981  // numerical Jacobian
982  // contains entire columns of zeros, even if the theorethical doesn't.
983  // check_jacob_LnT_T(1.0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) ); ...
984 }
985 
986 TEST_F(Pose3DTests, Jacob_dexpeD_de)
987 {
988  for (const auto& p : ptc) test_Jacob_dexpeD_de(p);
989 }
990 
991 TEST_F(Pose3DTests, Jacob_dDexpe_de)
992 {
993  for (const auto& p : ptc) test_Jacob_dDexpe_de(p);
994 }
995 
996 TEST_F(Pose3DTests, Jacob_dAexpeD_de)
997 {
998  for (const auto& p1 : ptc)
999  for (const auto& p2 : ptc) test_Jacob_dAexpeD_de(p1, p2);
1000 }
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 composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr, 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...
Definition: CPose3D.cpp:367
void test_invComposePointJacob_se3(const CPose3D &p, const TPoint3D x_g)
static const std::vector< mrpt::poses::CPose3D > ptc
double DEG2RAD(const double x)
Degrees to radians.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
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:542
STL namespace.
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:590
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 inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:646
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:562
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:84
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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)
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
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:519
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: 449735050 Mon Aug 19 09:11:33 2019 +0200 at lun ago 19 09:20:11 CEST 2019