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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019