MRPT  1.9.9
CPose3D_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <mrpt/poses/CPose2D.h>
11 #include <mrpt/poses/CPose3D.h>
12 #include <mrpt/poses/CPoint3D.h>
13 #include <mrpt/math/num_jacobian.h>
14 #include <CTraitsTest.h>
15 #include <gtest/gtest.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::poses;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 template class mrpt::CTraitsTest<CPose3D>;
23 
24 class Pose3DTests : public ::testing::Test
25 {
26  protected:
27  virtual void SetUp() {}
28  virtual void TearDown() {}
30  double x1, double y1, double z1, double yaw1, double pitch1,
31  double roll1)
32  {
33  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
34 
35  const CMatrixDouble44 HM =
37  CMatrixDouble44 HMi;
39 
40  CMatrixDouble44 I4;
41  I4.unit(4, 1.0);
42 
43  EXPECT_NEAR((HM * HMi - I4).array().abs().sum(), 0, 1e-3)
44  << "HM:\n"
45  << HM << "inv(HM):\n"
46  << HMi << "inv(HM)*HM:\n"
47  << HM * HMi << endl;
48 
49  CPose3D p1_inv_inv = p1;
50 
51  p1_inv_inv.inverse();
52  const CMatrixDouble44 HMi_from_p1_inv =
54 
55  p1_inv_inv.inverse();
56 
57  EXPECT_NEAR(
58  (p1.getAsVectorVal() - p1_inv_inv.getAsVectorVal())
59  .array()
60  .abs()
61  .sum(),
62  0, 1e-3)
63  << "p1: " << p1 << "p1_inv_inv: " << p1_inv_inv << endl;
64 
65  EXPECT_NEAR((HMi_from_p1_inv - HMi).array().abs().sum(), 0, 1e-4)
66  << "HMi_from_p1_inv:\n"
67  << HMi_from_p1_inv << "HMi:\n"
68  << HMi << endl;
69  }
70 
72  double x1, double y1, double z1, double yaw1, double pitch1,
73  double roll1, double x2, double y2, double z2, double yaw2,
74  double pitch2, double roll2)
75  {
76  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
77  const CPose3D p2(x2, y2, z2, yaw2, pitch2, roll2);
78 
79  const CPose3D p1_c_p2 = p1 + p2;
80  const CPose3D p1_i_p2 = p1 - p2;
81 
82  const CPose3D p1_c_p2_i_p2 = p1_c_p2 - p1; // should be -> p2
83  const CPose3D p2_c_p1_i_p2 = p2 + p1_i_p2; // Should be -> p1
84 
85  EXPECT_NEAR(
86  0,
87  (p1_c_p2_i_p2.getAsVectorVal() - p2.getAsVectorVal())
88  .array()
89  .abs()
90  .sum(),
91  1e-5)
92  << "p2 : " << p2 << endl
93  << "p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl;
94 
95  EXPECT_NEAR(
96  0,
97  (p2_c_p1_i_p2.getAsVectorVal() - p1.getAsVectorVal())
98  .array()
99  .abs()
100  .sum(),
101  1e-5)
102  << "p1 : " << p1 << endl
103  << "p2 : " << p2 << endl
104  << "p2 matrix : " << endl
106  << "p1_i_p2 : " << p1_i_p2 << endl
107  << "p1_i_p2 matrix: " << endl
108  << p1_i_p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
109  << "p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl;
110 
111  // Test + operator: trg new var
112  {
113  CPose3D C = p1;
114  CPose3D A = C + p2;
115  EXPECT_NEAR(
116  0,
117  (A.getAsVectorVal() - p1_c_p2.getAsVectorVal())
118  .array()
119  .abs()
120  .sum(),
121  1e-6);
122  }
123  // Test + operator: trg same var
124  {
125  CPose3D A = p1;
126  A = A + p2;
127  EXPECT_NEAR(
128  0,
129  (A.getAsVectorVal() - p1_c_p2.getAsVectorVal())
130  .array()
131  .abs()
132  .sum(),
133  1e-6);
134  }
135  // Test =+ operator
136  {
137  CPose3D A = p1;
138  A += p2;
139  EXPECT_NEAR(
140  0,
141  (A.getAsVectorVal() - p1_c_p2.getAsVectorVal())
142  .array()
143  .abs()
144  .sum(),
145  1e-6);
146  }
147  }
148 
149  void test_to_from_2d(double x, double y, double phi)
150  {
151  const CPose2D p2d = CPose2D(x, y, phi);
152  const CPose3D p3d = CPose3D(p2d);
153 
154  const CPose2D p2d_bis = CPose2D(p3d);
155 
156  EXPECT_FLOAT_EQ(p2d.x(), p2d_bis.x()) << "p2d: " << p2d << endl;
157  EXPECT_FLOAT_EQ(p2d.y(), p2d_bis.y()) << "p2d: " << p2d << endl;
158  EXPECT_FLOAT_EQ(p2d.phi(), p2d_bis.phi()) << "p2d: " << p2d << endl;
159 
160  EXPECT_FLOAT_EQ(p2d.phi(), p3d.yaw()) << "p2d: " << p2d << endl;
161  }
162 
164  double x1, double y1, double z1, double yaw1, double pitch1,
165  double roll1, double x2, double y2, double z2, double yaw2,
166  double pitch2, double roll2)
167  {
168  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
169  const CPose3D p2(x2, y2, z2, yaw2, pitch2, roll2);
170 
171  const CPose3D p1_plus_p2 = p1 + p2;
172 
173  {
174  CPose3D p1_plus_p2bis;
175  p1_plus_p2bis.composeFrom(p1, p2);
176 
177  EXPECT_NEAR(
178  0,
179  (p1_plus_p2bis.getAsVectorVal() - p1_plus_p2.getAsVectorVal())
180  .array()
181  .abs()
182  .sum(),
183  1e-5)
184  << "p2 : " << p2 << endl
185  << "p1 : " << p1 << endl
186  << "p1_plus_p2 : " << p1_plus_p2 << endl
187  << "p1_plus_p2bis : " << p1_plus_p2bis << endl;
188  }
189 
190  {
191  CPose3D p1_plus_p2bis = p1;
192  p1_plus_p2bis.composeFrom(p1_plus_p2bis, p2);
193 
194  EXPECT_NEAR(
195  0,
196  (p1_plus_p2bis.getAsVectorVal() - p1_plus_p2.getAsVectorVal())
197  .array()
198  .abs()
199  .sum(),
200  1e-5)
201  << "p2 : " << p2 << endl
202  << "p1 : " << p1 << endl
203  << "p1_plus_p2 : " << p1_plus_p2 << endl
204  << "p1_plus_p2bis : " << p1_plus_p2bis << endl;
205  }
206 
207  {
208  CPose3D p1_plus_p2bis = p2;
209  p1_plus_p2bis.composeFrom(p1, p1_plus_p2bis);
210 
211  EXPECT_NEAR(
212  0,
213  (p1_plus_p2bis.getAsVectorVal() - p1_plus_p2.getAsVectorVal())
214  .array()
215  .abs()
216  .sum(),
217  1e-5)
218  << "p2 : " << p2 << endl
219  << "p1 : " << p1 << endl
220  << "p1_plus_p2 : " << p1_plus_p2 << endl
221  << "p1_plus_p2bis : " << p1_plus_p2bis << endl;
222  }
223  }
224 
226  double x1, double y1, double z1, double yaw1, double pitch1,
227  double roll1, double x, double y, double z)
228  {
229  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
230  const CPoint3D p(x, y, z);
231  CPoint3D p1_plus_p = p1 + p;
232 
233  CPoint3D p1_plus_p2;
234  p1.composePoint(
235  p.x(), p.y(), p.z(), p1_plus_p2.x(), p1_plus_p2.y(),
236  p1_plus_p2.z());
237 
238  EXPECT_NEAR(
239  0,
240  (p1_plus_p2.getAsVectorVal() - p1_plus_p.getAsVectorVal())
241  .array()
242  .abs()
243  .sum(),
244  1e-5);
245 
246  // Repeat using same input/output variables:
247  {
248  double lx = p.x();
249  double ly = p.y();
250  double lz = p.z();
251 
252  p1.composePoint(lx, ly, lz, lx, ly, lz);
253  EXPECT_NEAR(
254  0,
255  std::abs(lx - p1_plus_p.x()) + std::abs(ly - p1_plus_p.y()) +
256  std::abs(lz - p1_plus_p.z()),
257  1e-5);
258  }
259 
260  // Inverse:
261  CPoint3D p_recov = p1_plus_p - p1;
262  CPoint3D p_recov2;
263  p1.inverseComposePoint(
264  p1_plus_p.x(), p1_plus_p.y(), p1_plus_p.z(), p_recov2.x(),
265  p_recov2.y(), p_recov2.z());
266 
267  EXPECT_NEAR(
268  0,
269  (p_recov2.getAsVectorVal() - p_recov.getAsVectorVal())
270  .array()
271  .abs()
272  .sum(),
273  1e-5);
274 
275  EXPECT_NEAR(
276  0,
277  (p.getAsVectorVal() - p_recov.getAsVectorVal()).array().abs().sum(),
278  1e-5);
279  }
280 
281  static void func_compose_point(
282  const CArrayDouble<6 + 3>& x, const double& dummy, CArrayDouble<3>& Y)
283  {
284  MRPT_UNUSED_PARAM(dummy);
285  CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
286  const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
287  const CPoint3D pp = q + p;
288  for (int i = 0; i < 3; i++) Y[i] = pp[i];
289  }
290 
292  const CArrayDouble<6 + 3>& x, const double& dummy, CArrayDouble<3>& Y)
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  double x1, double y1, double z1, double yaw1, double pitch1,
305  double roll1, double x, double y, double z, bool use_aprox = false)
306  {
307  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
308  const CPoint3D p(x, y, z);
309 
312 
313  TPoint3D pp;
314  p1.composePoint(
315  x, y, z, pp.x, pp.y, pp.z, &df_dpoint, &df_dpose, nullptr,
316  use_aprox);
317 
318  // Numerical approx:
321  {
322  CArrayDouble<6 + 3> x_mean;
323  for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
324  x_mean[6 + 0] = x;
325  x_mean[6 + 1] = y;
326  x_mean[6 + 2] = z;
327 
328  double DUMMY = 0;
329  CArrayDouble<6 + 3> x_incrs;
330  x_incrs.assign(1e-7);
331  CMatrixDouble numJacobs;
333  x_mean,
334  std::function<void(
335  const CArrayDouble<6 + 3>& x, const double& dummy,
336  CArrayDouble<3>& Y)>(&func_compose_point),
337  x_incrs, DUMMY, numJacobs);
338 
339  numJacobs.extractMatrix(0, 0, num_df_dpose);
340  numJacobs.extractMatrix(0, 6, num_df_dpoint);
341  }
342 
343  const double max_eror = use_aprox ? 0.1 : 3e-3;
344 
345  EXPECT_NEAR(
346  0, (df_dpoint - num_df_dpoint).array().abs().sum(), max_eror)
347  << "p1: " << p1 << endl
348  << "p: " << p << endl
349  << "Numeric approximation of df_dpoint: " << endl
350  << num_df_dpoint << endl
351  << "Implemented method: " << endl
352  << df_dpoint << endl
353  << "Error: " << endl
354  << df_dpoint - num_df_dpoint << endl;
355 
356  EXPECT_NEAR(0, (df_dpose - num_df_dpose).array().abs().sum(), max_eror)
357  << "p1: " << p1 << endl
358  << "p: " << p << endl
359  << "Numeric approximation of df_dpose: " << endl
360  << num_df_dpose << endl
361  << "Implemented method: " << endl
362  << df_dpose << endl
363  << "Error: " << endl
364  << df_dpose - num_df_dpose << endl;
365  }
366 
368  double x1, double y1, double z1, double yaw1, double pitch1,
369  double roll1)
370  {
371  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
372 
373  const CPose3D p2 = CPose3D::exp(p1.ln());
374  EXPECT_NEAR(
375  (p1.getAsVectorVal() - p2.getAsVectorVal()).array().abs().sum(), 0,
376  1e-5)
377  << "p1: " << p1 << endl;
378  }
379 
381  double x1, double y1, double z1, double yaw1, double pitch1,
382  double roll1, double x, double y, double z)
383  {
384  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
385  const CPoint3D p(x, y, z);
386 
389 
390  TPoint3D pp;
392  x, y, z, pp.x, pp.y, pp.z, &df_dpoint, &df_dpose);
393 
394  // Numerical approx:
397  {
398  CArrayDouble<6 + 3> x_mean;
399  for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
400  x_mean[6 + 0] = x;
401  x_mean[6 + 1] = y;
402  x_mean[6 + 2] = z;
403 
404  double DUMMY = 0;
405  CArrayDouble<6 + 3> x_incrs;
406  x_incrs.assign(1e-7);
407  CMatrixDouble numJacobs;
409  x_mean,
410  std::function<void(
411  const CArrayDouble<6 + 3>& x, const double& dummy,
412  CArrayDouble<3>& Y)>(&func_inv_compose_point),
413  x_incrs, DUMMY, numJacobs);
414 
415  numJacobs.extractMatrix(0, 0, num_df_dpose);
416  numJacobs.extractMatrix(0, 6, num_df_dpoint);
417  }
418 
419  EXPECT_NEAR(0, (df_dpoint - num_df_dpoint).array().abs().sum(), 3e-3)
420  << "p1: " << p1 << endl
421  << "p: " << p << endl
422  << "Numeric approximation of df_dpoint: " << endl
423  << num_df_dpoint << endl
424  << "Implemented method: " << endl
425  << df_dpoint << endl
426  << "Error: " << endl
427  << df_dpoint - num_df_dpoint << endl;
428 
429  EXPECT_NEAR(0, (df_dpose - num_df_dpose).array().abs().sum(), 3e-3)
430  << "p1: " << p1 << endl
431  << "p: " << p << endl
432  << "Numeric approximation of df_dpose: " << endl
433  << num_df_dpose << endl
434  << "Implemented method: " << endl
435  << df_dpose << endl
436  << "Error: " << endl
437  << df_dpose - num_df_dpose << endl;
438  }
439 
440  void test_default_values(const CPose3D& p, const std::string& label)
441  {
442  EXPECT_EQ(p.x(), 0);
443  EXPECT_EQ(p.y(), 0);
444  EXPECT_EQ(p.z(), 0);
445  EXPECT_EQ(p.yaw(), 0);
446  EXPECT_EQ(p.pitch(), 0);
447  EXPECT_EQ(p.roll(), 0);
448  for (size_t i = 0; i < 4; i++)
449  for (size_t j = 0; j < 4; j++)
450  EXPECT_NEAR(
451  p.getHomogeneousMatrixVal<CMatrixDouble44>()(i, j),
452  i == j ? 1.0 : 0.0, 1e-8)
453  << "Failed for (i,j)=" << i << "," << j << endl
454  << "Matrix is: " << endl
455  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
456  << "case was: " << label << endl;
457  }
458 
460  const CArrayDouble<6>& x, const CArrayDouble<3>& P, CArrayDouble<3>& Y)
461  {
462  CPose3D q = CPose3D::exp(x);
463  const CPoint3D p(P[0], P[1], P[2]);
464  const CPoint3D pp = q + p;
465  for (int i = 0; i < 3; i++) Y[i] = pp[i];
466  }
467 
469  const CArrayDouble<6>& x, const CArrayDouble<3>& P, CArrayDouble<3>& Y)
470  {
471  CPose3D q = CPose3D::exp(x);
472  const CPoint3D p(P[0], P[1], P[2]);
473  const CPoint3D pp = p - q;
474  for (int i = 0; i < 3; i++) Y[i] = pp[i];
475  }
476 
478  {
480 
481  TPoint3D pp;
482  p.composePoint(
483  x_l.x, x_l.y, x_l.z, pp.x, pp.y, pp.z, nullptr, nullptr, &df_dse3);
484 
485  // Numerical approx:
487  {
488  CArrayDouble<6> x_mean;
489  for (int i = 0; i < 6; i++) x_mean[i] = 0;
490 
491  CArrayDouble<3> P;
492  for (int i = 0; i < 3; i++) P[i] = pp[i];
493 
494  CArrayDouble<6> x_incrs;
495  x_incrs.assign(1e-9);
497  x_mean,
498  std::function<void(
499  const CArrayDouble<6>& x, const CArrayDouble<3>& P,
500  CArrayDouble<3>& Y)>(&func_compose_point_se3),
501  x_incrs, P, num_df_dse3);
502  }
503 
504  EXPECT_NEAR(0, (df_dse3 - num_df_dse3).array().abs().sum(), 3e-3)
505  << "p: " << p << endl
506  << "x_l: " << x_l << endl
507  << "Numeric approximation of df_dse3: " << endl
508  << num_df_dse3 << endl
509  << "Implemented method: " << endl
510  << df_dse3 << endl
511  << "Error: " << endl
512  << df_dse3 - num_df_dse3 << endl;
513  }
514 
516  {
518 
519  TPoint3D pp;
520  p.inverseComposePoint(
521  x_g.x, x_g.y, x_g.z, pp.x, pp.y, pp.z, nullptr, nullptr, &df_dse3);
522 
523  // Numerical approx:
525  {
526  CArrayDouble<6> x_mean;
527  for (int i = 0; i < 6; i++) x_mean[i] = 0;
528 
529  CArrayDouble<3> P;
530  for (int i = 0; i < 3; i++) P[i] = pp[i];
531 
532  CArrayDouble<6> x_incrs;
533  x_incrs.assign(1e-9);
535  x_mean,
536  std::function<void(
537  const CArrayDouble<6>& x, const CArrayDouble<3>& P,
538  CArrayDouble<3>& Y)>(&func_invcompose_point_se3),
539  x_incrs, P, num_df_dse3);
540  }
541 
542  EXPECT_NEAR(0, (df_dse3 - num_df_dse3).array().abs().sum(), 3e-3)
543  << "p: " << p << endl
544  << "x_g: " << x_g << endl
545  << "Numeric approximation of df_dse3: " << endl
546  << num_df_dse3 << endl
547  << "Implemented method: " << endl
548  << df_dse3 << endl
549  << "Error: " << endl
550  << df_dse3 - num_df_dse3 << endl;
551  }
552 
553  static void func_jacob_expe_e(
554  const CArrayDouble<6>& x, const double& dummy, CArrayDouble<12>& Y)
555  {
556  MRPT_UNUSED_PARAM(dummy);
557  const CPose3D p = CPose3D::exp(x);
558  // const CMatrixDouble44 R =
559  // p.getHomogeneousMatrixVal<CMatrixDouble44>();
560  p.getAs12Vector(Y);
561  }
562 
563  // Check dexp(e)_de
565  {
566  CArrayDouble<6> x_mean;
567  for (int i = 0; i < 6; i++) x_mean[i] = 0;
568 
569  double dummy = 0.;
570  CArrayDouble<6> x_incrs;
571  x_incrs.assign(1e-9);
572  CMatrixDouble numJacobs;
574  x_mean,
575  std::function<void(
576  const CArrayDouble<6>& x, const double& dummy,
577  CArrayDouble<12>& Y)>(&func_jacob_expe_e),
578  x_incrs, dummy, numJacobs);
579 
580  // Theoretical matrix:
581  // [ 0 -[e1]_x ]
582  // [ 0 -[e2]_x ]
583  // [ 0 -[e3]_x ]
584  // [ I_3 0 ]
585  double vals[12 * 6] = {
586  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0,
587 
588  0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
589 
590  0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0,
591 
592  1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0};
594 
595  EXPECT_NEAR((numJacobs - M).array().abs().maxCoeff(), 0, 1e-5)
596  << "M:\n"
597  << M << "numJacobs:\n"
598  << numJacobs << "\n";
599  }
600 
601  static void func_jacob_LnT_T(
602  const CArrayDouble<12>& x, const double& dummy, CArrayDouble<6>& Y)
603  {
604  MRPT_UNUSED_PARAM(dummy);
605  CPose3D p;
606 
607  p.setFrom12Vector(x);
608  // ensure 3x3 rot vector is orthonormal (Sophus complains otherwise):
609  auto R = p.getRotationMatrix();
610  const auto Rsvd =
611  R.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
612  R = Rsvd.matrixU() * Rsvd.matrixV().transpose();
613  p.setRotationMatrix(R);
614 
615  Y = p.ln();
616  }
617 
618  // Jacobian of Ln(T) wrt T
620  double x1, double y1, double z1, double yaw1, double pitch1,
621  double roll1)
622  {
623  const CPose3D p(x1, y1, z1, yaw1, pitch1, roll1);
624 
626  p.ln_jacob(theor_jacob);
627 
628  CMatrixDouble numJacobs;
629  {
630  CArrayDouble<12> x_mean;
631  p.getAs12Vector(x_mean);
632 
633  double dummy = 0.;
634  CArrayDouble<12> x_incrs;
635  x_incrs.assign(1e-6);
637  x_mean,
638  std::function<void(
639  const CArrayDouble<12>& x, const double& dummy,
640  CArrayDouble<6>& Y)>(&func_jacob_LnT_T),
641  x_incrs, dummy, numJacobs);
642  }
643 
644  EXPECT_NEAR((numJacobs - theor_jacob).array().abs().sum(), 0, 1e-3)
645  << "Pose: " << p << endl
646  << "Pose matrix:\n"
647  << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
648  << numJacobs << endl
649  << "Theor. Jacob:\n"
650  << theor_jacob << endl
651  << "ERR:\n"
652  << theor_jacob - numJacobs << endl;
653  }
654 
655  // tech report:
656  //
657  static void func_jacob_expe_D(
658  const CArrayDouble<6>& eps, const CPose3D& D, CArrayDouble<12>& Y)
659  {
660  CPose3D incr;
661  CPose3D::exp(eps, incr);
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  double x1, double y1, double z1, double yaw1, double pitch1,
670  double roll1)
671  {
672  const CPose3D p(x1, y1, z1, yaw1, pitch1, roll1);
673 
674  Eigen::Matrix<double, 12, 6> theor_jacob;
675  CPose3D::jacob_dexpeD_de(p, theor_jacob);
676 
677  CMatrixDouble numJacobs;
678  {
679  CArrayDouble<6> x_mean;
680  x_mean.setZero();
681 
682  CArrayDouble<6> x_incrs;
683  x_incrs.assign(1e-6);
685  x_mean,
686  std::function<void(
687  const CArrayDouble<6>& eps, const CPose3D& D,
688  CArrayDouble<12>& Y)>(&func_jacob_expe_D),
689  x_incrs, p, numJacobs);
690  }
691 
692  EXPECT_NEAR((numJacobs - theor_jacob).array().abs().maxCoeff(), 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 << endl
699  << "ERR:\n"
700  << theor_jacob - numJacobs << endl;
701  }
702 
704  {
705  CPose3D A, D;
706  };
707 
708  static void func_jacob_Aexpe_D(
710  CArrayDouble<12>& Y)
711  {
712  CPose3D incr;
713  CPose3D::exp(eps, incr);
714  const CPose3D res = params.A + incr + params.D;
715  res.getAs12Vector(Y);
716  }
717 
718  // Test Jacobian: d A*exp(e)*D / d e
719  // 10.3.7 in tech report
720  // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
722  double x1, double y1, double z1, double yaw1, double pitch1,
723  double roll1, double x2, double y2, double z2, double yaw2,
724  double pitch2, double roll2)
725  {
726  const CPose3D A(x1, y1, z1, yaw1, pitch1, roll1);
727  const CPose3D D(x2, y2, z2, yaw2, pitch2, roll2);
728 
729  Eigen::Matrix<double, 12, 6> theor_jacob;
730  CPose3D::jacob_dAexpeD_de(A, D, theor_jacob);
731 
732  CMatrixDouble numJacobs;
733  {
734  CArrayDouble<6> x_mean;
735  x_mean.setZero();
736 
738  params.A = A;
739  params.D = D;
740  CArrayDouble<6> x_incrs;
741  x_incrs.assign(1e-6);
743  x_mean,
744  std::function<void(
745  const CArrayDouble<6>& eps,
747  CArrayDouble<12>& Y)>(&func_jacob_Aexpe_D),
748  x_incrs, params, numJacobs);
749  }
750 
751  EXPECT_NEAR((numJacobs - theor_jacob).array().abs().maxCoeff(), 0, 1e-3)
752  << "Pose A: " << A << endl
753  << "Pose D: " << D << endl
754  << "Num. Jacob:\n"
755  << numJacobs << endl
756  << "Theor. Jacob:\n"
757  << theor_jacob << endl
758  << "ERR:\n"
759  << theor_jacob - numJacobs << endl;
760  }
761 };
762 
763 // Elemental tests:
764 TEST_F(Pose3DTests, DefaultValues)
765 {
766  {
767  CPose3D p;
768  test_default_values(p, "Default");
769  }
770  {
771  CPose3D p2;
772  CPose3D p = p2;
773  test_default_values(p, "p=p2");
774  }
775  {
776  CPose3D p1, p2;
777  test_default_values(p1 + p2, "p1+p2");
778  CPose3D p = p1 + p2;
779  test_default_values(p, "p=p1+p2");
780  }
781  {
782  CPose3D p1, p2;
783  CPose3D p = p1 - p2;
784  test_default_values(p, "p1-p2");
785  }
786 }
787 
788 TEST_F(Pose3DTests, Initialization)
789 {
790  CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
791  EXPECT_NEAR(p.x(), 1, 1e-7);
792  EXPECT_NEAR(p.y(), 2, 1e-7);
793  EXPECT_NEAR(p.z(), 3, 1e-7);
794  EXPECT_NEAR(p.yaw(), 0.2, 1e-7);
795  EXPECT_NEAR(p.pitch(), 0.3, 1e-7);
796  EXPECT_NEAR(p.roll(), 0.4, 1e-7);
797 }
798 
799 TEST_F(Pose3DTests, OperatorBracket)
800 {
801  CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
802  EXPECT_NEAR(p[0], 1, 1e-7);
803  EXPECT_NEAR(p[1], 2, 1e-7);
804  EXPECT_NEAR(p[2], 3, 1e-7);
805  EXPECT_NEAR(p[3], 0.2, 1e-7);
806  EXPECT_NEAR(p[4], 0.3, 1e-7);
807  EXPECT_NEAR(p[5], 0.4, 1e-7);
808 }
809 
810 // List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in
811 // degrees)
812 const double ptc[][6] = { // ptc = poses_test_cases
813  {.0, .0, .0, .0, .0, .0},
814  {1.0, 2.0, 3.0, .0, .0, .0},
815  {1.0, 2.0, 3.0, 10.0, .0, .0},
816  {1.0, 2.0, 3.0, .0, 1.0, .0},
817  {1.0, 2.0, 3.0, .0, .0, 1.0},
818  {1.0, 2.0, 3.0, 80.0, 5.0, 5.0},
819  {1.0, 2.0, 3.0, -20.0, -30.0, -40.0},
820  {1.0, 2.0, 3.0, -45.0, 10.0, 70.0},
821  {1.0, 2.0, 3.0, 40.0, -5.0, 25.0},
822  {1.0, 2.0, 3.0, 40.0, 20.0, -15.0},
823  {-6.0, 2.0, 3.0, 40.0, 20.0, 15.0},
824  {6.0, -5.0, 3.0, 40.0, 20.0, 15.0},
825  {6.0, 2.0, -9.0, 40.0, 20.0, 15.0},
826  {0.0, 8.0, 5.0, -45.0, 10.0, 70.0},
827  {1.0, 0.0, 5.0, -45.0, 10.0, 70.0},
828  {1.0, 8.0, 0.0, -45.0, 10.0, 70.0}};
829 const size_t num_ptc = sizeof(ptc) / sizeof(ptc[0]);
830 
831 // More complex tests:
832 TEST_F(Pose3DTests, InverseHM)
833 {
834  for (size_t i = 0; i < num_ptc; i++)
835  test_inverse(
836  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
837  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]));
838 }
839 
841 {
842  for (size_t i = 0; i < num_ptc; i++)
843  for (size_t j = 0; j < num_ptc; j++)
844  test_compose(
845  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
846  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), ptc[j][0], ptc[j][1],
847  ptc[j][2], DEG2RAD(ptc[j][3]), DEG2RAD(ptc[j][4]),
848  DEG2RAD(ptc[j][5]));
849 }
850 TEST_F(Pose3DTests, composeFrom)
851 {
852  for (size_t i = 0; i < num_ptc; i++)
853  for (size_t j = 0; j < num_ptc; j++)
854  test_composeFrom(
855  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
856  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), ptc[j][0], ptc[j][1],
857  ptc[j][2], DEG2RAD(ptc[j][3]), DEG2RAD(ptc[j][4]),
858  DEG2RAD(ptc[j][5]));
859 }
860 
861 TEST_F(Pose3DTests, ToFromCPose2D)
862 {
863  for (size_t i = 0; i < num_ptc; i++)
864  test_to_from_2d(ptc[i][0], ptc[i][1], DEG2RAD(ptc[i][3]));
865 }
866 
867 TEST_F(Pose3DTests, ComposeAndInvComposeWithPoint)
868 {
869  for (size_t i = 0; i < num_ptc; i++)
870  {
871  test_composePoint(
872  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
873  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 10, 11, 12);
874  test_composePoint(
875  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
876  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), -5, 1, 2);
877  test_composePoint(
878  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
879  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 5, -1, 2);
880  test_composePoint(
881  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
882  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 5, 1, -2);
883  }
884 }
885 
886 TEST_F(Pose3DTests, ComposePointJacob)
887 {
888  for (size_t i = 0; i < num_ptc; i++)
889  {
890  test_composePointJacob(
891  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
892  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 10, 11, 12);
893  test_composePointJacob(
894  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
895  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), -5, 1, 2);
896  }
897 }
898 
899 TEST_F(Pose3DTests, ComposePointJacobApprox)
900 { // Test approximated Jacobians for very small rotations
901  test_composePointJacob(
902  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12, true);
903  test_composePointJacob(
904  1.0, 2.0, 3.0, DEG2RAD(0.1), DEG2RAD(0), DEG2RAD(0), 10, 11, 12, true);
905  test_composePointJacob(
906  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0.1), DEG2RAD(0), 10, 11, 12, true);
907  test_composePointJacob(
908  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0.1), 10, 11, 12, true);
909 }
910 
911 TEST_F(Pose3DTests, InvComposePointJacob)
912 {
913  for (size_t i = 0; i < num_ptc; i++)
914  {
915  test_invComposePointJacob(
916  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
917  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 10, 11, 12);
918  test_invComposePointJacob(
919  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
920  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), -5, 1, 2);
921  test_invComposePointJacob(
922  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
923  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 5, -1, 2);
924  test_invComposePointJacob(
925  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
926  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 5, 1, -2);
927  }
928 }
929 
930 TEST_F(Pose3DTests, ComposePointJacob_se3)
931 {
932  for (size_t i = 0; i < num_ptc; i++)
933  {
934  test_composePointJacob_se3(
935  CPose3D(
936  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
937  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
938  TPoint3D(0, 0, 0));
939  test_composePointJacob_se3(
940  CPose3D(
941  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
942  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
943  TPoint3D(10, 11, 12));
944  test_composePointJacob_se3(
945  CPose3D(
946  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
947  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
948  TPoint3D(-5.0, -15.0, 8.0));
949  }
950 }
951 TEST_F(Pose3DTests, InvComposePointJacob_se3)
952 {
953  for (size_t i = 0; i < num_ptc; i++)
954  {
955  test_invComposePointJacob_se3(
956  CPose3D(
957  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
958  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
959  TPoint3D(0, 0, 0));
960  test_invComposePointJacob_se3(
961  CPose3D(
962  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
963  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
964  TPoint3D(10, 11, 12));
965  test_invComposePointJacob_se3(
966  CPose3D(
967  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
968  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
969  TPoint3D(-5.0, -15.0, 8.0));
970  }
971 }
972 
973 TEST_F(Pose3DTests, ExpLnEqual)
974 {
975  for (size_t i = 0; i < num_ptc; i++)
976  test_ExpLnEqual(
977  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
978  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]));
979 }
980 
981 TEST_F(Pose3DTests, Jacob_dExpe_de_at_0) { check_jacob_expe_e_at_0(); }
982 TEST_F(Pose3DTests, Jacob_dLnT_dT)
983 {
984  check_jacob_LnT_T(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
985  // JL NOTE:
986  // This function cannot be properly tested numerically, since the logm()
987  // implementation
988  // is not generic and does NOT depends on all matrix entries, thus the
989  // numerical Jacobian
990  // contains entire columns of zeros, even if the theorethical doesn't.
991  // check_jacob_LnT_T(1.0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) ); ...
992 }
993 
994 TEST_F(Pose3DTests, Jacob_dexpeD_de)
995 {
996  for (size_t i = 0; i < num_ptc; i++)
997  test_Jacob_dexpeD_de(
998  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
999  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]));
1000 }
1001 
1002 TEST_F(Pose3DTests, Jacob_dAexpeD_de)
1003 {
1004  for (size_t i = 0; i < num_ptc; i++)
1005  for (size_t j = 0; j < num_ptc; j++)
1006  test_Jacob_dAexpeD_de(
1007  ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
1008  DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), ptc[j][0], ptc[j][1],
1009  ptc[j][2], DEG2RAD(ptc[j][3]), DEG2RAD(ptc[j][4]),
1010  DEG2RAD(ptc[j][5]));
1011 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
TEST_F(Pose3DTests, DefaultValues)
static void func_jacob_expe_D(const CArrayDouble< 6 > &eps, const CPose3D &D, CArrayDouble< 12 > &Y)
GLdouble GLdouble z
Definition: glext.h:3872
void test_composeFrom(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
void test_Jacob_dexpeD_de(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
mrpt::math::CVectorDouble getAsVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:263
void test_invComposePointJacob_se3(const CPose3D &p, const TPoint3D x_g)
void test_Jacob_dAexpeD_de(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
double DEG2RAD(const double x)
Degrees to radians.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
void test_composePointJacob_se3(const CPose3D &p, const TPoint3D x_l)
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:526
STL namespace.
void test_ExpLnEqual(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:593
void test_to_from_2d(double x, double y, double phi)
void test_composePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
void check_jacob_expe_e_at_0()
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:29
static void jacob_dexpeD_de(const CPose3D &D, Eigen::Matrix< double, 12, 6 > &jacob)
The Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.
Definition: CPose3D.cpp:997
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
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:565
void test_invComposePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
static void func_compose_point(const CArrayDouble< 6+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
const double eps
double x
X,Y,Z coordinates.
virtual void TearDown()
GLsizei const GLchar ** string
Definition: glext.h:4101
static void func_compose_point_se3(const CArrayDouble< 6 > &x, const CArrayDouble< 3 > &P, CArrayDouble< 3 > &Y)
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...
static void func_inv_compose_point(const CArrayDouble< 6+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static void func_jacob_LnT_T(const CArrayDouble< 12 > &x, const double &dummy, CArrayDouble< 6 > &Y)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< 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:379
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:275
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:80
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
Definition: CPoseOrPoint.h:287
GLenum GLint GLint y
Definition: glext.h:3538
static void func_jacob_Aexpe_D(const CArrayDouble< 6 > &eps, const TParams_func_jacob_Aexpe_D &params, CArrayDouble< 12 > &Y)
void check_jacob_LnT_T(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
static void func_jacob_expe_e(const CArrayDouble< 6 > &x, const double &dummy, CArrayDouble< 12 > &Y)
const size_t num_ptc
virtual void SetUp()
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
void test_compose(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
static void func_invcompose_point_se3(const CArrayDouble< 6 > &x, const CArrayDouble< 3 > &P, CArrayDouble< 3 > &Y)
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:503
void ln(mrpt::math::CArrayDouble< 6 > &out_ln) const
Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the...
Definition: CPose3D.cpp:801
static CPose3D exp(const mrpt::math::CArrayNumeric< double, 6 > &vect, bool pseudo_exponential=false)
Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D (static method).
Definition: CPose3D.cpp:758
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
void test_inverse(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
static void jacob_dAexpeD_de(const CPose3D &A, const CPose3D &D, Eigen::Matrix< double, 12, 6 > &jacob)
The Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie Algebra.
Definition: CPose3D.cpp:1017
void test_default_values(const CPose3D &p, const std::string &label)
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:649
const double ptc[][6]
void test_composePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z, bool use_aprox=false)
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020