Main MRPT website > C++ reference for 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() {}
29  void test_inverse(
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 =
36  p1.getHomogeneousMatrixVal<CMatrixDouble44>();
37  CMatrixDouble44 HMi;
38  p1.getInverseHomogeneousMatrix(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 =
53  p1_inv_inv.getHomogeneousMatrixVal<CMatrixDouble44>();
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 
71  void test_compose(
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
105  << p2.getHomogeneousMatrixVal<CMatrixDouble44>() << 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 
163  void test_composeFrom(
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 
225  void test_composePoint(
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 
291  static void func_inv_compose_point(
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 
303  void test_composePointJacob(
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 
310  CMatrixFixedNumeric<double, 3, 3> df_dpoint;
311  CMatrixFixedNumeric<double, 3, 6> df_dpose;
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:
319  CMatrixFixedNumeric<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
320  CMatrixFixedNumeric<double, 3, 6> num_df_dpose(UNINITIALIZED_MATRIX);
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;
332  mrpt::math::estimateJacobian(
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 
367  void test_ExpLnEqual(
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 
380  void test_invComposePointJacob(
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 
387  CMatrixFixedNumeric<double, 3, 3> df_dpoint;
388  CMatrixFixedNumeric<double, 3, 6> df_dpose;
389 
390  TPoint3D pp;
391  p1.inverseComposePoint(
392  x, y, z, pp.x, pp.y, pp.z, &df_dpoint, &df_dpose);
393 
394  // Numerical approx:
395  CMatrixFixedNumeric<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
396  CMatrixFixedNumeric<double, 3, 6> num_df_dpose(UNINITIALIZED_MATRIX);
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;
408  mrpt::math::estimateJacobian(
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 
459  static void func_compose_point_se3(
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 
468  static void func_invcompose_point_se3(
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 
477  void test_composePointJacob_se3(const CPose3D& p, const TPoint3D x_l)
478  {
479  CMatrixFixedNumeric<double, 3, 6> df_dse3;
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:
486  CMatrixFixedNumeric<double, 3, 6> num_df_dse3(UNINITIALIZED_MATRIX);
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);
496  mrpt::math::estimateJacobian(
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 
515  void test_invComposePointJacob_se3(const CPose3D& p, const TPoint3D x_g)
516  {
517  CMatrixFixedNumeric<double, 3, 6> df_dse3;
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:
524  CMatrixFixedNumeric<double, 3, 6> num_df_dse3(UNINITIALIZED_MATRIX);
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);
534  mrpt::math::estimateJacobian(
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
564  void check_jacob_expe_e_at_0()
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;
573  mrpt::math::estimateJacobian(
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};
593  CMatrixFixedNumeric<double, 12, 6> M(vals);
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
619  void check_jacob_LnT_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 
625  CMatrixFixedNumeric<double, 6, 12> theor_jacob;
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);
636  mrpt::math::estimateJacobian(
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
668  void test_Jacob_dexpeD_de(
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);
684  mrpt::math::estimateJacobian(
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 
703  struct TParams_func_jacob_Aexpe_D
704  {
705  CPose3D A, D;
706  };
707 
708  static void func_jacob_Aexpe_D(
709  const CArrayDouble<6>& eps, const TParams_func_jacob_Aexpe_D& params,
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
721  void test_Jacob_dAexpeD_de(
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 
737  TParams_func_jacob_Aexpe_D params;
738  params.A = A;
739  params.D = D;
740  CArrayDouble<6> x_incrs;
741  x_incrs.assign(1e-6);
742  mrpt::math::estimateJacobian(
743  x_mean,
744  std::function<void(
745  const CArrayDouble<6>& eps,
746  const TParams_func_jacob_Aexpe_D& params,
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 
840 TEST_F(Pose3DTests, Compose)
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 }
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
mrpt::poses::CPose3D::jacob_dexpeD_de
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:1000
Pose3DTests::func_jacob_expe_D
static void func_jacob_expe_D(const CArrayDouble< 6 > &eps, const CPose3D &D, CArrayDouble< 12 > &Y)
Definition: CPose3D_unittest.cpp:657
Pose3DTests::TearDown
virtual void TearDown()
Definition: CPose3D_unittest.cpp:28
mrpt::poses::CPose3D::composeFrom
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
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
Pose3DTests::func_invcompose_point_se3
static void func_invcompose_point_se3(const CArrayDouble< 6 > &x, const CArrayDouble< 3 > &P, CArrayDouble< 3 > &Y)
Definition: CPose3D_unittest.cpp:468
Pose3DTests::test_composeFrom
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)
Definition: CPose3D_unittest.cpp:163
mrpt::poses::CPose2D::phi
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
mrpt::math::TPoint3D::z
double z
Definition: lightweight_geom_data.h:385
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
Pose3DTests::func_jacob_LnT_T
static void func_jacob_LnT_T(const CArrayDouble< 12 > &x, const double &dummy, CArrayDouble< 6 > &Y)
Definition: CPose3D_unittest.cpp:601
Pose3DTests::test_invComposePointJacob
void test_invComposePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3D_unittest.cpp:380
R
const float R
Definition: CKinematicChain.cpp:138
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
CPose2D.h
mrpt::poses::CPose3D::ln
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
TEST_F
TEST_F(Pose3DTests, DefaultValues)
Definition: CPose3D_unittest.cpp:764
ptc
const double ptc[][6]
Definition: CPose3D_unittest.cpp:812
Pose3DTests::test_Jacob_dexpeD_de
void test_Jacob_dexpeD_de(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3D_unittest.cpp:668
Pose3DTests::SetUp
virtual void SetUp()
Definition: CPose3D_unittest.cpp:27
mrpt::math::CMatrixTemplateNumeric< double >
eps
const double eps
Definition: distributions_unittest.cpp:19
mrpt::poses::CPoseOrPoint::getAsVectorVal
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
mrpt::poses::CPose3D::getAs12Vector
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:505
Pose3DTests::func_compose_point_se3
static void func_compose_point_se3(const CArrayDouble< 6 > &x, const CArrayDouble< 3 > &P, CArrayDouble< 3 > &Y)
Definition: CPose3D_unittest.cpp:459
num_ptc
const size_t num_ptc
Definition: CPose3D_unittest.cpp:829
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:144
num_jacobian.h
Pose3DTests::test_compose
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)
Definition: CPose3D_unittest.cpp:71
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:40
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
res
GLuint res
Definition: glext.h:7268
mrpt::math::TPoint3D::x
double x
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:385
mrpt::poses::CPose3D::exp
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
Pose3DTests::func_jacob_Aexpe_D
static void func_jacob_Aexpe_D(const CArrayDouble< 6 > &eps, const TParams_func_jacob_Aexpe_D &params, CArrayDouble< 12 > &Y)
Definition: CPose3D_unittest.cpp:708
Pose3DTests::test_composePointJacob
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)
Definition: CPose3D_unittest.cpp:303
mrpt::math::CArrayNumeric
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
Definition: CArrayNumeric.h:25
mrpt::poses::CPoseOrPoint::getInverseHomogeneousMatrix
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose.
Definition: CPoseOrPoint.h:287
mrpt::poses::CPoseOrPoint::getHomogeneousMatrixVal
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:275
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:40
CPoint3D.h
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:378
Pose3DTests::check_jacob_LnT_T
void check_jacob_LnT_T(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3D_unittest.cpp:619
CPose3D.h
Pose3DTests::test_invComposePointJacob_se3
void test_invComposePointJacob_se3(const CPose3D &p, const TPoint3D x_g)
Definition: CPose3D_unittest.cpp:515
mrpt::math::TPoint3D::y
double y
Definition: lightweight_geom_data.h:385
mrpt::math::UNINITIALIZED_MATRIX
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
mrpt::poses::CPose3D::inverse
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:593
mrpt::poses::CPose3D::composePoint
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
mrpt::poses::CPose3D::jacob_dAexpeD_de
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:1020
Pose3DTests::TParams_func_jacob_Aexpe_D::D
CPose3D D
Definition: CPose3D_unittest.cpp:705
Pose3DTests::check_jacob_expe_e_at_0
void check_jacob_expe_e_at_0()
Definition: CPose3D_unittest.cpp:564
Pose3DTests::test_Jacob_dAexpeD_de
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)
Definition: CPose3D_unittest.cpp:721
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:528
Pose3DTests::TParams_func_jacob_Aexpe_D
Definition: CPose3D_unittest.cpp:703
Pose3DTests::test_ExpLnEqual
void test_ExpLnEqual(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3D_unittest.cpp:367
mrpt::math::estimateJacobian
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
Pose3DTests
Definition: CPose3D_unittest.cpp:24
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
z
GLdouble GLdouble z
Definition: glext.h:3872
Pose3DTests::test_to_from_2d
void test_to_from_2d(double x, double y, double phi)
Definition: CPose3D_unittest.cpp:149
Pose3DTests::test_composePointJacob_se3
void test_composePointJacob_se3(const CPose3D &p, const TPoint3D x_l)
Definition: CPose3D_unittest.cpp:477
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::poses::CPose3D::inverseComposePoint
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
Pose3DTests::func_inv_compose_point
static void func_inv_compose_point(const CArrayDouble< 6+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
Definition: CPose3D_unittest.cpp:291
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:33
Pose3DTests::func_jacob_expe_e
static void func_jacob_expe_e(const CArrayDouble< 6 > &x, const double &dummy, CArrayDouble< 12 > &Y)
Definition: CPose3D_unittest.cpp:553
y
GLenum GLint GLint y
Definition: glext.h:3538
Pose3DTests::func_compose_point
static void func_compose_point(const CArrayDouble< 6+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
Definition: CPose3D_unittest.cpp:281
x
GLenum GLint x
Definition: glext.h:3538
params
GLenum const GLfloat * params
Definition: glext.h:3534
Pose3DTests::test_inverse
void test_inverse(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3D_unittest.cpp:29
Pose3DTests::test_composePoint
void test_composePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3D_unittest.cpp:225
Pose3DTests::test_default_values
void test_default_values(const CPose3D &p, const std::string &label)
Definition: CPose3D_unittest.cpp:440
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST