Main MRPT website > C++ reference for MRPT 1.5.7
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 <gtest/gtest.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::poses;
16 using namespace mrpt::utils;
17 using namespace mrpt::math;
18 using namespace std;
19 
20 class Pose3DTests : public ::testing::Test {
21 protected:
22  virtual void SetUp()
23  {
24  }
25 
26  virtual void TearDown() { }
27 
28  void test_inverse(double x1,double y1,double z1, double yaw1,double pitch1,double roll1)
29  {
30  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
31 
32  const CMatrixDouble44 HM = p1.getHomogeneousMatrixVal();
33  const CMatrixDouble44 HMi = p1.getInverseHomogeneousMatrix();
34 
35  CMatrixDouble44 I4; I4.unit(4,1.0);
36 
37  EXPECT_NEAR( (HM*HMi-I4).array().abs().sum(), 0, 1e-3 ) <<
38  "HM:\n" << HM <<
39  "inv(HM):\n" << HMi <<
40  "inv(HM)*HM:\n" << HM*HMi << endl;
41 
42  CPose3D p1_inv_inv = p1;
43 
44  p1_inv_inv.inverse();
45  const CMatrixDouble44 HMi_from_p1_inv = p1_inv_inv.getHomogeneousMatrixVal();
46 
47  p1_inv_inv.inverse();
48 
49  EXPECT_NEAR( (p1.getAsVectorVal()-p1_inv_inv.getAsVectorVal()).array().abs().sum(), 0, 1e-3 ) <<
50  "p1: " << p1 <<
51  "p1_inv_inv: " << p1_inv_inv << endl;
52 
53  EXPECT_NEAR((HMi_from_p1_inv-HMi).array().abs().sum(),0, 1e-4)
54  << "HMi_from_p1_inv:\n" << HMi_from_p1_inv
55  << "HMi:\n" << HMi << endl;
56  }
57 
58 
59  void test_compose(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
60  double x2,double y2,double z2, double yaw2,double pitch2,double roll2 )
61  {
62  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
63  const CPose3D p2(x2,y2,z2,yaw2,pitch2,roll2);
64 
65  const CPose3D p1_c_p2 = p1 + p2;
66  const CPose3D p1_i_p2 = p1 - p2;
67 
68  const CPose3D p1_c_p2_i_p2 = p1_c_p2 - p1; // should be -> p2
69  const CPose3D p2_c_p1_i_p2 = p2 + p1_i_p2; // Should be -> p1
70 
71  EXPECT_NEAR(0, (p1_c_p2_i_p2.getAsVectorVal()-p2.getAsVectorVal()).array().abs().sum(), 1e-5)
72  << "p2 : " << p2 << endl
73  << "p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl;
74 
75  EXPECT_NEAR(0, (p2_c_p1_i_p2.getAsVectorVal()-p1.getAsVectorVal()).array().abs().sum(), 1e-5)
76  << "p1 : " << p1 << endl
77  << "p2 : " << p2 << endl
78  << "p2 matrix : " << endl << p2.getHomogeneousMatrixVal() << endl
79  << "p1_i_p2 : " << p1_i_p2 << endl
80  << "p1_i_p2 matrix: " << endl << p1_i_p2.getHomogeneousMatrixVal() << endl
81  << "p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl;
82 
83  // Test + operator: trg new var
84  {
85  CPose3D C = p1;
86  CPose3D A = C + p2;
87  EXPECT_NEAR(0, (A.getAsVectorVal()-p1_c_p2.getAsVectorVal()).array().abs().sum(), 1e-6);
88  }
89  // Test + operator: trg same var
90  {
91  CPose3D A = p1;
92  A = A + p2;
93  EXPECT_NEAR(0, (A.getAsVectorVal()-p1_c_p2.getAsVectorVal()).array().abs().sum(), 1e-6);
94  }
95  // Test =+ operator
96  {
97  CPose3D A = p1;
98  A += p2;
99  EXPECT_NEAR(0, (A.getAsVectorVal()-p1_c_p2.getAsVectorVal()).array().abs().sum(), 1e-6);
100  }
101  }
102 
103  void test_to_from_2d(double x,double y, double phi)
104  {
105  const CPose2D p2d = CPose2D(x,y,phi);
106  const CPose3D p3d = CPose3D(p2d);
107 
108  const CPose2D p2d_bis = CPose2D(p3d);
109 
110  EXPECT_FLOAT_EQ( p2d.x(), p2d_bis.x() ) << "p2d: " << p2d << endl;
111  EXPECT_FLOAT_EQ( p2d.y(), p2d_bis.y() ) << "p2d: " << p2d << endl;
112  EXPECT_FLOAT_EQ( p2d.phi(), p2d_bis.phi() ) << "p2d: " << p2d << endl;
113 
114  EXPECT_FLOAT_EQ( p2d.phi(), p3d.yaw() ) << "p2d: " << p2d << endl;
115  }
116 
117  void test_composeFrom(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
118  double x2,double y2,double z2, double yaw2,double pitch2,double roll2 )
119  {
120  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
121  const CPose3D p2(x2,y2,z2,yaw2,pitch2,roll2);
122 
123  const CPose3D p1_plus_p2 = p1 + p2;
124 
125  {
126  CPose3D p1_plus_p2bis;
127  p1_plus_p2bis.composeFrom(p1,p2);
128 
129  EXPECT_NEAR(0, (p1_plus_p2bis.getAsVectorVal()-p1_plus_p2.getAsVectorVal()).array().abs().sum(), 1e-5)
130  << "p2 : " << p2 << endl
131  << "p1 : " << p1 << endl
132  << "p1_plus_p2 : " << p1_plus_p2 << endl
133  << "p1_plus_p2bis : " << p1_plus_p2bis<< endl;
134  }
135 
136  {
137  CPose3D p1_plus_p2bis = p1;
138  p1_plus_p2bis.composeFrom(p1_plus_p2bis,p2);
139 
140  EXPECT_NEAR(0, (p1_plus_p2bis.getAsVectorVal()-p1_plus_p2.getAsVectorVal()).array().abs().sum(), 1e-5)
141  << "p2 : " << p2 << endl
142  << "p1 : " << p1 << endl
143  << "p1_plus_p2 : " << p1_plus_p2 << endl
144  << "p1_plus_p2bis : " << p1_plus_p2bis<< endl;
145  }
146 
147  {
148  CPose3D p1_plus_p2bis = p2;
149  p1_plus_p2bis.composeFrom(p1,p1_plus_p2bis);
150 
151  EXPECT_NEAR(0, (p1_plus_p2bis.getAsVectorVal()-p1_plus_p2.getAsVectorVal()).array().abs().sum(), 1e-5)
152  << "p2 : " << p2 << endl
153  << "p1 : " << p1 << endl
154  << "p1_plus_p2 : " << p1_plus_p2 << endl
155  << "p1_plus_p2bis : " << p1_plus_p2bis<< endl;
156  }
157  }
158 
159  void test_composePoint(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
160  double x,double y,double z)
161  {
162  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
163  const CPoint3D p(x,y,z);
164  CPoint3D p1_plus_p = p1 + p;
165 
166  CPoint3D p1_plus_p2;
167  p1.composePoint(p.x(),p.y(),p.z() ,p1_plus_p2.x(), p1_plus_p2.y(), p1_plus_p2.z());
168 
169  EXPECT_NEAR(0, (p1_plus_p2.getAsVectorVal()-p1_plus_p.getAsVectorVal()).array().abs().sum(), 1e-5);
170 
171  // Repeat using same input/output variables:
172  {
173  double x = p.x();
174  double y = p.y();
175  double z = p.z();
176 
177  p1.composePoint(x,y,z, x,y,z);
178  EXPECT_NEAR(0, std::abs(x-p1_plus_p.x())+std::abs(y-p1_plus_p.y())+std::abs(z-p1_plus_p.z()) , 1e-5);
179  }
180 
181  // Inverse:
182  CPoint3D p_recov = p1_plus_p - p1;
183  CPoint3D p_recov2;
184  p1.inverseComposePoint(p1_plus_p.x(),p1_plus_p.y(),p1_plus_p.z(), p_recov2.x(),p_recov2.y(),p_recov2.z() );
185 
186  EXPECT_NEAR(0, (p_recov2.getAsVectorVal()-p_recov.getAsVectorVal()).array().abs().sum(), 1e-5);
187 
188  EXPECT_NEAR(0, (p.getAsVectorVal()-p_recov.getAsVectorVal()).array().abs().sum(), 1e-5);
189  }
190 
191  static void func_compose_point(const CArrayDouble<6+3> &x, const double &dummy, CArrayDouble<3> &Y)
192  {
193  MRPT_UNUSED_PARAM(dummy);
194  CPose3D q(x[0],x[1],x[2],x[3],x[4],x[5]);
195  const CPoint3D p(x[6+0],x[6+1],x[6+2]);
196  const CPoint3D pp = q+p;
197  for (int i=0;i<3;i++) Y[i]=pp[i];
198  }
199 
200  static void func_inv_compose_point(const CArrayDouble<6+3> &x, const double &dummy, CArrayDouble<3> &Y)
201  {
202  MRPT_UNUSED_PARAM(dummy);
203  CPose3D q(x[0],x[1],x[2],x[3],x[4],x[5]);
204  const CPoint3D p(x[6+0],x[6+1],x[6+2]);
205  const CPoint3D pp = p-q;
206  Y[0]=pp.x();
207  Y[1]=pp.y();
208  Y[2]=pp.z();
209  }
210 
211  void test_composePointJacob(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
212  double x,double y,double z, bool use_aprox = false)
213  {
214  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
215  const CPoint3D p(x,y,z);
216 
217  CMatrixFixedNumeric<double,3,3> df_dpoint;
218  CMatrixFixedNumeric<double,3,6> df_dpose;
219 
220  TPoint3D pp;
221  p1.composePoint(x,y,z, pp.x,pp.y,pp.z, &df_dpoint, &df_dpose, NULL, use_aprox );
222 
223  // Numerical approx:
224  CMatrixFixedNumeric<double,3,3> num_df_dpoint(UNINITIALIZED_MATRIX);
225  CMatrixFixedNumeric<double,3,6> num_df_dpose(UNINITIALIZED_MATRIX);
226  {
227  CArrayDouble<6+3> x_mean;
228  for (int i=0;i<6;i++) x_mean[i]=p1[i];
229  x_mean[6+0]=x;
230  x_mean[6+1]=y;
231  x_mean[6+2]=z;
232 
233  double DUMMY=0;
234  CArrayDouble<6+3> x_incrs;
235  x_incrs.assign(1e-7);
236  CMatrixDouble numJacobs;
237  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose_point,x_incrs, DUMMY, numJacobs );
238 
239  numJacobs.extractMatrix(0,0, num_df_dpose);
240  numJacobs.extractMatrix(0,6, num_df_dpoint);
241  }
242 
243  const double max_eror = use_aprox ? 0.1 : 3e-3;
244 
245  EXPECT_NEAR(0, (df_dpoint-num_df_dpoint).array().abs().sum(), max_eror )
246  << "p1: " << p1 << endl
247  << "p: " << p << endl
248  << "Numeric approximation of df_dpoint: " << endl << num_df_dpoint << endl
249  << "Implemented method: " << endl << df_dpoint << endl
250  << "Error: " << endl << df_dpoint-num_df_dpoint << endl;
251 
252  EXPECT_NEAR(0, (df_dpose-num_df_dpose).array().abs().sum(), max_eror )
253  << "p1: " << p1 << endl
254  << "p: " << p << endl
255  << "Numeric approximation of df_dpose: " << endl << num_df_dpose << endl
256  << "Implemented method: " << endl << df_dpose << endl
257  << "Error: " << endl << df_dpose-num_df_dpose << endl;
258  }
259 
260 
261  void test_ExpLnEqual(double x1,double y1,double z1, double yaw1,double pitch1,double roll1)
262  {
263  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
264 
265  const CPose3D p2 = CPose3D::exp( p1.ln() );
266  EXPECT_NEAR((p1.getAsVectorVal()-p2.getAsVectorVal()).array().abs().sum(),0, 1e-5 ) << "p1: " << p1 <<endl;
267  }
268 
269 
270  void test_invComposePointJacob(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
271  double x,double y,double z)
272  {
273  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
274  const CPoint3D p(x,y,z);
275 
276  CMatrixFixedNumeric<double,3,3> df_dpoint;
277  CMatrixFixedNumeric<double,3,6> df_dpose;
278 
279  TPoint3D pp;
280  p1.inverseComposePoint(x,y,z, pp.x,pp.y,pp.z, &df_dpoint, &df_dpose );
281 
282  // Numerical approx:
283  CMatrixFixedNumeric<double,3,3> num_df_dpoint(UNINITIALIZED_MATRIX);
284  CMatrixFixedNumeric<double,3,6> num_df_dpose(UNINITIALIZED_MATRIX);
285  {
286  CArrayDouble<6+3> x_mean;
287  for (int i=0;i<6;i++) x_mean[i]=p1[i];
288  x_mean[6+0]=x;
289  x_mean[6+1]=y;
290  x_mean[6+2]=z;
291 
292  double DUMMY=0;
293  CArrayDouble<6+3> x_incrs;
294  x_incrs.assign(1e-7);
295  CMatrixDouble numJacobs;
296  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_inv_compose_point,x_incrs, DUMMY, numJacobs );
297 
298  numJacobs.extractMatrix(0,0, num_df_dpose);
299  numJacobs.extractMatrix(0,6, num_df_dpoint);
300  }
301 
302  EXPECT_NEAR(0, (df_dpoint-num_df_dpoint).array().abs().sum(), 3e-3 )
303  << "p1: " << p1 << endl
304  << "p: " << p << endl
305  << "Numeric approximation of df_dpoint: " << endl << num_df_dpoint << endl
306  << "Implemented method: " << endl << df_dpoint << endl
307  << "Error: " << endl << df_dpoint-num_df_dpoint << endl;
308 
309  EXPECT_NEAR(0, (df_dpose-num_df_dpose).array().abs().sum(), 3e-3 )
310  << "p1: " << p1 << endl
311  << "p: " << p << endl
312  << "Numeric approximation of df_dpose: " << endl << num_df_dpose << endl
313  << "Implemented method: " << endl << df_dpose << endl
314  << "Error: " << endl << df_dpose-num_df_dpose << endl;
315  }
316 
317 
318  void test_default_values(const CPose3D &p, const std::string & label)
319  {
320  EXPECT_EQ(p.x(),0);
321  EXPECT_EQ(p.y(),0);
322  EXPECT_EQ(p.z(),0);
323  EXPECT_EQ(p.yaw(),0);
324  EXPECT_EQ(p.pitch(),0);
325  EXPECT_EQ(p.roll(),0);
326  for (size_t i=0;i<4;i++)
327  for (size_t j=0;j<4;j++)
328  EXPECT_NEAR(p.getHomogeneousMatrixVal()(i,j), i==j ? 1.0 : 0.0, 1e-8 )
329  << "Failed for (i,j)=" << i << "," << j << endl
330  << "Matrix is: " << endl << p.getHomogeneousMatrixVal() << endl
331  << "case was: " << label << endl;
332  }
333 
334  static void func_compose_point_se3(const CArrayDouble<6> &x, const CArrayDouble<3> &P, CArrayDouble<3> &Y)
335  {
336  CPose3D q = CPose3D::exp(x);
337  const CPoint3D p(P[0],P[1],P[2]);
338  const CPoint3D pp = q+p;
339  for (int i=0;i<3;i++) Y[i]=pp[i];
340  }
341 
342  static void func_invcompose_point_se3(const CArrayDouble<6> &x, const CArrayDouble<3> &P, CArrayDouble<3> &Y)
343  {
344  CPose3D q = CPose3D::exp(x);
345  const CPoint3D p(P[0],P[1],P[2]);
346  const CPoint3D pp = p-q;
347  for (int i=0;i<3;i++) Y[i]=pp[i];
348  }
349 
350 
351  void test_composePointJacob_se3( const CPose3D &p, const TPoint3D x_l )
352  {
353  CMatrixFixedNumeric<double,3,6> df_dse3;
354 
355  TPoint3D pp;
356  p.composePoint(x_l.x,x_l.y,x_l.z, pp.x,pp.y,pp.z, NULL, NULL, &df_dse3);
357 
358  // Numerical approx:
359  CMatrixFixedNumeric<double,3,6> num_df_dse3(UNINITIALIZED_MATRIX);
360  {
361  CArrayDouble<6> x_mean;
362  for (int i=0;i<6;i++) x_mean[i]=0;
363 
364  CArrayDouble<3> P;
365  for (int i=0;i<3;i++) P[i]=pp[i];
366 
367  CArrayDouble<6> x_incrs;
368  x_incrs.assign(1e-9);
369  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose_point_se3,x_incrs, P, num_df_dse3 );
370  }
371 
372  EXPECT_NEAR(0, (df_dse3-num_df_dse3).array().abs().sum(), 3e-3 )
373  << "p: " << p << endl
374  << "x_l: " << x_l << endl
375  << "Numeric approximation of df_dse3: " << endl <<num_df_dse3 << endl
376  << "Implemented method: " << endl << df_dse3 << endl
377  << "Error: " << endl << df_dse3-num_df_dse3 << endl;
378  }
379 
380  void test_invComposePointJacob_se3( const CPose3D &p, const TPoint3D x_g )
381  {
382  CMatrixFixedNumeric<double,3,6> df_dse3;
383 
384  TPoint3D pp;
385  p.inverseComposePoint(x_g.x,x_g.y,x_g.z, pp.x,pp.y,pp.z, NULL, NULL, &df_dse3 );
386 
387  // Numerical approx:
388  CMatrixFixedNumeric<double,3,6> num_df_dse3(UNINITIALIZED_MATRIX);
389  {
390  CArrayDouble<6> x_mean;
391  for (int i=0;i<6;i++) x_mean[i]=0;
392 
393  CArrayDouble<3> P;
394  for (int i=0;i<3;i++) P[i]=pp[i];
395 
396  CArrayDouble<6> x_incrs;
397  x_incrs.assign(1e-9);
398  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_invcompose_point_se3,x_incrs, P, num_df_dse3 );
399  }
400 
401  EXPECT_NEAR(0, (df_dse3-num_df_dse3).array().abs().sum(), 3e-3 )
402  << "p: " << p << endl
403  << "x_g: " << x_g << endl
404  << "Numeric approximation of df_dse3: " << endl <<num_df_dse3 << endl
405  << "Implemented method: " << endl << df_dse3 << endl
406  << "Error: " << endl << df_dse3-num_df_dse3 << endl;
407  }
408 
409  static void func_jacob_expe_e(
410  const CArrayDouble<6> &x,
411  const double &dummy, CArrayDouble<12> &Y)
412  {
413  MRPT_UNUSED_PARAM(dummy);
414  const CPose3D p = CPose3D::exp(x);
415  //const CMatrixDouble44 R = p.getHomogeneousMatrixVal();
416  p.getAs12Vector(Y);
417  }
418 
419  // Check dexp(e)_de
420  void check_jacob_expe_e_at_0()
421  {
422  CArrayDouble<6> x_mean;
423  for (int i=0;i<6;i++) x_mean[i]=0;
424 
425  double dummy=0.;
426  CArrayDouble<6> x_incrs;
427  x_incrs.assign(1e-9);
428  CMatrixDouble numJacobs;
429  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_jacob_expe_e,x_incrs,dummy, numJacobs );
430 
431  // Theoretical matrix:
432  // [ 0 -[e1]_x ]
433  // [ 0 -[e2]_x ]
434  // [ 0 -[e3]_x ]
435  // [ I_3 0 ]
436  double vals[12*6] = {
437  0, 0, 0, 0, 0, 0,
438  0, 0, 0, 0, 0, 1,
439  0, 0, 0, 0,-1, 0,
440 
441  0, 0, 0, 0, 0,-1,
442  0, 0, 0, 0, 0, 0,
443  0, 0, 0, 1, 0, 0,
444 
445  0, 0, 0, 0, 1, 0,
446  0, 0, 0,-1, 0, 0,
447  0, 0, 0, 0, 0, 0,
448 
449  1, 0, 0, 0, 0, 0,
450  0, 1, 0, 0, 0, 0,
451  0, 0, 1, 0, 0, 0
452  };
453  CMatrixFixedNumeric<double,12,6> M(vals);
454 
455  EXPECT_NEAR( (numJacobs-M).array().abs().maxCoeff(), 0, 1e-5) << "M:\n" << M << "numJacobs:\n" << numJacobs << "\n";
456  }
457 
458  static void func_jacob_LnT_T(
459  const CArrayDouble<12> &x,
460  const double &dummy, CArrayDouble<6> &Y)
461  {
462  MRPT_UNUSED_PARAM(dummy);
463  CPose3D p;
464  p.setFrom12Vector(x);
465  Y = p.ln();
466  }
467 
468  // Jacobian of Ln(T) wrt T
469  void check_jacob_LnT_T(double x1,double y1,double z1, double yaw1,double pitch1,double roll1)
470  {
471  const CPose3D p(x1,y1,z1,yaw1,pitch1,roll1);
472 
473  CMatrixFixedNumeric<double,6,12> theor_jacob;
474  p.ln_jacob(theor_jacob);
475 
476  CMatrixDouble numJacobs;
477  {
478  CArrayDouble<12> x_mean;
479  p.getAs12Vector(x_mean);
480 
481  double dummy=0.;
482  CArrayDouble<12> x_incrs;
483  x_incrs.assign(1e-6);
484  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_jacob_LnT_T,x_incrs,dummy, numJacobs );
485  }
486 
487  EXPECT_NEAR( (numJacobs-theor_jacob).array().abs().sum(), 0, 1e-3)
488  << "Pose: " << p << endl
489  << "Pose matrix:\n" << p.getHomogeneousMatrixVal()
490  << "Num. Jacob:\n" << numJacobs << endl
491  << "Theor. Jacob:\n" << theor_jacob << endl
492  << "ERR:\n" << theor_jacob-numJacobs << endl;
493  }
494 
495  // tech report:
496  //
497  static void func_jacob_expe_D(
498  const CArrayDouble<6> &eps,
499  const CPose3D &D, CArrayDouble<12> &Y)
500  {
501  CPose3D incr;
502  CPose3D::exp(eps,incr);
503  const CPose3D expe_D = incr + D;
504  expe_D.getAs12Vector(Y);
505  }
506 
507 
508  // Test Jacobian: d exp(e)*D / d e
509  // 10.3.3 in tech report
510  void test_Jacob_dexpeD_de(double x1,double y1,double z1, double yaw1,double pitch1,double roll1)
511  {
512  const CPose3D p(x1,y1,z1,yaw1,pitch1,roll1);
513 
514  Eigen::Matrix<double,12,6> theor_jacob;
515  CPose3D::jacob_dexpeD_de(p,theor_jacob);
516 
517  CMatrixDouble numJacobs;
518  {
519  CArrayDouble<6> x_mean;
520  x_mean.setZero();
521 
522  CArrayDouble<6> x_incrs;
523  x_incrs.assign(1e-6);
524  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_jacob_expe_D,x_incrs,p, numJacobs );
525  }
526 
527  EXPECT_NEAR( (numJacobs-theor_jacob).array().abs().maxCoeff(), 0, 1e-3)
528  << "Pose: " << p << endl
529  << "Pose matrix:\n" << p.getHomogeneousMatrixVal()
530  << "Num. Jacob:\n" << numJacobs << endl
531  << "Theor. Jacob:\n" << theor_jacob << endl
532  << "ERR:\n" << theor_jacob-numJacobs << endl;
533  }
534 
535 
536 
537  struct TParams_func_jacob_Aexpe_D
538  {
539  CPose3D A,D;
540  };
541 
542  static void func_jacob_Aexpe_D(
543  const CArrayDouble<6> &eps,
544  const TParams_func_jacob_Aexpe_D &params, CArrayDouble<12> &Y)
545  {
546  CPose3D incr;
547  CPose3D::exp(eps,incr);
548  const CPose3D res = params.A + incr + params.D;
549  res.getAs12Vector(Y);
550  }
551 
552 
553  // Test Jacobian: d A*exp(e)*D / d e
554  // 10.3.7 in tech report http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
555  void test_Jacob_dAexpeD_de(
556  double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
557  double x2,double y2,double z2, double yaw2,double pitch2,double roll2)
558  {
559  const CPose3D A(x1,y1,z1,yaw1,pitch1,roll1);
560  const CPose3D D(x2,y2,z2,yaw2,pitch2,roll2);
561 
562  Eigen::Matrix<double,12,6> theor_jacob;
563  CPose3D::jacob_dAexpeD_de(A,D,theor_jacob);
564 
565  CMatrixDouble numJacobs;
566  {
567  CArrayDouble<6> x_mean;
568  x_mean.setZero();
569 
570  TParams_func_jacob_Aexpe_D params;
571  params.A=A;
572  params.D=D;
573  CArrayDouble<6> x_incrs;
574  x_incrs.assign(1e-6);
575  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_jacob_Aexpe_D,x_incrs,params, numJacobs );
576  }
577 
578  EXPECT_NEAR( (numJacobs-theor_jacob).array().abs().maxCoeff(), 0, 1e-3)
579  << "Pose A: " << A << endl
580  << "Pose D: " << D << endl
581  << "Num. Jacob:\n" << numJacobs << endl
582  << "Theor. Jacob:\n" << theor_jacob << endl
583  << "ERR:\n" << theor_jacob-numJacobs << endl;
584  }
585 
586 
587 };
588 
589 // Elemental tests:
590 TEST_F(Pose3DTests,DefaultValues)
591 {
592  {
593  CPose3D p;
594  test_default_values(p, "Default");
595  }
596  {
597  CPose3D p2;
598  CPose3D p = p2;
599  test_default_values(p, "p=p2");
600  }
601  {
602  CPose3D p1,p2;
603  test_default_values(p1+p2, "p1+p2");
604  CPose3D p = p1+p2;
605  test_default_values(p, "p=p1+p2");
606  }
607  {
608  CPose3D p1,p2;
609  CPose3D p = p1-p2;
610  test_default_values(p,"p1-p2");
611  }
612 }
613 
614 TEST_F(Pose3DTests,Initialization)
615 {
616  CPose3D p(1,2,3,0.2,0.3,0.4);
617  EXPECT_NEAR(p.x(),1, 1e-7);
618  EXPECT_NEAR(p.y(),2, 1e-7);
619  EXPECT_NEAR(p.z(),3, 1e-7);
620  EXPECT_NEAR(p.yaw(),0.2, 1e-7);
621  EXPECT_NEAR(p.pitch(),0.3, 1e-7);
622  EXPECT_NEAR(p.roll(),0.4, 1e-7);
623 }
624 
625 TEST_F(Pose3DTests,OperatorBracket)
626 {
627  CPose3D p(1,2,3,0.2,0.3,0.4);
628  EXPECT_NEAR(p[0],1, 1e-7);
629  EXPECT_NEAR(p[1],2, 1e-7);
630  EXPECT_NEAR(p[2],3, 1e-7);
631  EXPECT_NEAR(p[3],0.2, 1e-7);
632  EXPECT_NEAR(p[4],0.3, 1e-7);
633  EXPECT_NEAR(p[5],0.4, 1e-7);
634 }
635 
636 
637 // List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in degrees)
638 const double ptc[][6] = { // ptc = poses_test_cases
639  { .0, .0, .0, .0, .0, .0 },
640  { 1.0, 2.0, 3.0, .0, .0, .0 },
641  { 1.0, 2.0, 3.0, 10.0, .0, .0 },
642  { 1.0, 2.0, 3.0, .0, 1.0, .0 },
643  { 1.0, 2.0, 3.0, .0, .0, 1.0 },
644  { 1.0, 2.0, 3.0, 80.0, 5.0, 5.0 },
645  { 1.0, 2.0, 3.0,-20.0,-30.0,-40.0 },
646  { 1.0, 2.0, 3.0,-45.0, 10.0, 70.0 },
647  { 1.0, 2.0, 3.0, 40.0, -5.0, 25.0 },
648  { 1.0, 2.0, 3.0, 40.0, 20.0,-15.0 },
649  {-6.0, 2.0, 3.0, 40.0, 20.0, 15.0 },
650  { 6.0, -5.0, 3.0, 40.0, 20.0, 15.0 },
651  { 6.0, 2.0, -9.0, 40.0, 20.0, 15.0 },
652  { 0.0, 8.0, 5.0,-45.0, 10.0, 70.0 },
653  { 1.0, 0.0, 5.0,-45.0, 10.0, 70.0 },
654  { 1.0, 8.0, 0.0,-45.0, 10.0, 70.0 }
655 };
656 const size_t num_ptc = sizeof(ptc)/sizeof(ptc[0]);
657 
658 
659 // More complex tests:
660 TEST_F(Pose3DTests,InverseHM)
661 {
662  for (size_t i=0;i<num_ptc;i++)
663  test_inverse(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]) );
664 }
665 
666 TEST_F(Pose3DTests,Compose)
667 {
668  for (size_t i=0;i<num_ptc;i++)
669  for (size_t j=0;j<num_ptc;j++)
670  test_compose(
671  ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]),
672  ptc[j][0],ptc[j][1],ptc[j][2], DEG2RAD(ptc[j][3]),DEG2RAD(ptc[j][4]),DEG2RAD(ptc[j][5]) );
673 }
674 TEST_F(Pose3DTests,composeFrom)
675 {
676  for (size_t i=0;i<num_ptc;i++)
677  for (size_t j=0;j<num_ptc;j++)
678  test_composeFrom(
679  ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]),
680  ptc[j][0],ptc[j][1],ptc[j][2], DEG2RAD(ptc[j][3]),DEG2RAD(ptc[j][4]),DEG2RAD(ptc[j][5]) );
681 }
682 
683 TEST_F(Pose3DTests,ToFromCPose2D)
684 {
685  for (size_t i=0;i<num_ptc;i++)
686  test_to_from_2d( ptc[i][0],ptc[i][1], DEG2RAD(ptc[i][3]));
687 }
688 
689 TEST_F(Pose3DTests,ComposeAndInvComposeWithPoint)
690 {
691  for (size_t i=0;i<num_ptc;i++)
692  {
693  test_composePoint(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), 10,11,12);
694  test_composePoint(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), -5, 1, 2);
695  test_composePoint(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), 5,-1, 2);
696  test_composePoint(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), 5, 1,-2);
697  }
698 }
699 
700 TEST_F(Pose3DTests,ComposePointJacob)
701 {
702  for (size_t i=0;i<num_ptc;i++)
703  {
704  test_composePointJacob(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), 10,11,12);
705  test_composePointJacob(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), -5, 1, 2);
706  }
707 }
708 
709 TEST_F(Pose3DTests,ComposePointJacobApprox)
710 { // Test approximated Jacobians for very small rotations
711  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 10,11,12 , true );
712  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(0.1),DEG2RAD(0),DEG2RAD(0), 10,11,12 , true );
713  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0.1),DEG2RAD(0), 10,11,12 , true );
714  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0.1), 10,11,12 , true );
715 }
716 
717 TEST_F(Pose3DTests,InvComposePointJacob)
718 {
719  for (size_t i=0;i<num_ptc;i++)
720  {
721  test_invComposePointJacob(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), 10,11,12);
722  test_invComposePointJacob(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), -5, 1, 2);
723  test_invComposePointJacob(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), 5, -1, 2);
724  test_invComposePointJacob(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]), 5, 1, -2);
725  }
726 }
727 
728 TEST_F(Pose3DTests,ComposePointJacob_se3)
729 {
730  for (size_t i=0;i<num_ptc;i++)
731  {
732  test_composePointJacob_se3(CPose3D(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5])), TPoint3D(0,0,0 ) );
733  test_composePointJacob_se3(CPose3D(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5])), TPoint3D(10,11,12 ) );
734  test_composePointJacob_se3(CPose3D(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5])), TPoint3D(-5.0, -15.0, 8.0 ) );
735  }
736 }
737 TEST_F(Pose3DTests,InvComposePointJacob_se3)
738 {
739  for (size_t i=0;i<num_ptc;i++)
740  {
741  test_invComposePointJacob_se3(CPose3D(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5])), TPoint3D(0,0,0 ) );
742  test_invComposePointJacob_se3(CPose3D(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5])), TPoint3D(10,11,12 ) );
743  test_invComposePointJacob_se3(CPose3D(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5])), TPoint3D(-5.0, -15.0, 8.0 ) );
744  }
745 }
746 
747 TEST_F(Pose3DTests,ExpLnEqual)
748 {
749  for (size_t i=0;i<num_ptc;i++)
750  test_ExpLnEqual(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]));
751 }
752 
753 TEST_F(Pose3DTests,Jacob_dExpe_de_at_0)
754 {
755  check_jacob_expe_e_at_0();
756 }
757 
758 TEST_F(Pose3DTests,Jacob_dLnT_dT)
759 {
760  check_jacob_LnT_T(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
761  // JL NOTE:
762  // This function cannot be properly tested numerically, since the logm() implementation
763  // is not generic and does NOT depends on all matrix entries, thus the numerical Jacobian
764  // contains entire columns of zeros, even if the theorethical doesn't.
765 // check_jacob_LnT_T(1.0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) ); ...
766 }
767 
768 TEST_F(Pose3DTests,Jacob_dexpeD_de)
769 {
770  for (size_t i=0;i<num_ptc;i++)
771  test_Jacob_dexpeD_de(ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]));
772 }
773 
774 TEST_F(Pose3DTests,Jacob_dAexpeD_de)
775 {
776  for (size_t i=0;i<num_ptc;i++)
777  for (size_t j=0;j<num_ptc;j++)
778  test_Jacob_dAexpeD_de(
779  ptc[i][0],ptc[i][1],ptc[i][2], DEG2RAD(ptc[i][3]),DEG2RAD(ptc[i][4]),DEG2RAD(ptc[i][5]),
780  ptc[j][0],ptc[j][1],ptc[j][2], DEG2RAD(ptc[j][3]),DEG2RAD(ptc[j][4]),DEG2RAD(ptc[j][5]) );
781 }
782 
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
TEST_F
TEST_F(Pose3DTests, DefaultValues)
Definition: CPose3D_unittest.cpp:590
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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:497
z
GLdouble GLdouble z
Definition: glext.h:3734
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:117
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:510
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:181
Pose3DTests::test_invComposePointJacob_se3
void test_invComposePointJacob_se3(const CPose3D &p, const TPoint3D x_g)
Definition: CPose3D_unittest.cpp:380
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:555
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
Pose3DTests::test_composePointJacob_se3
void test_composePointJacob_se3(const CPose3D &p, const TPoint3D x_l)
Definition: CPose3D_unittest.cpp:351
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
std
STL namespace.
CPose3D.h
mrpt::math::TPoint3D::z
double z
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:248
Pose3DTests::test_ExpLnEqual
void test_ExpLnEqual(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3D_unittest.cpp:261
mrpt::poses::CPose3D::inverse
void inverse()
Convert this pose into its inverse, saving the result in itself.
Definition: CPose3D.cpp:618
Pose3DTests::test_to_from_2d
void test_to_from_2d(double x, double y, double phi)
Definition: CPose3D_unittest.cpp:103
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:159
Pose3DTests::check_jacob_expe_e_at_0
void check_jacob_expe_e_at_0()
Definition: CPose3D_unittest.cpp:420
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:988
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=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, 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:427
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:34
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: maps/CColouredPointsMap.h:307
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:595
mrpt::poses::CPose3D::getHomogeneousMatrixVal
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
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:270
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:201
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:167
Pose3DTests::func_compose_point
static void func_compose_point(const CArrayDouble< 6+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
Definition: CPose3D_unittest.cpp:191
Pose3DTests::TParams_func_jacob_Aexpe_D::D
CPose3D D
Definition: CPose3D_unittest.cpp:539
eps
const double eps
Definition: distributions_unittest.cpp:21
DEG2RAD
#define DEG2RAD
Definition: maps/CColouredPointsMap.h:99
Pose3DTests::TearDown
virtual void TearDown()
Definition: CPose3D_unittest.cpp:26
string
GLsizei const GLchar ** string
Definition: glext.h:3919
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:334
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:200
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
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=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:671
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:458
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::poses::CPose2D::phi
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
mrpt::math::CArrayDouble
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
Pose3DTests::TParams_func_jacob_Aexpe_D
Definition: CPose3D_unittest.cpp:537
y
GLenum GLint GLint y
Definition: glext.h:3516
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:542
mrpt::math::jacobians::jacob_numeric_estimate
void jacob_numeric_estimate(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), 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:119
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:469
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:409
num_ptc
const size_t num_ptc
Definition: CPose3D_unittest.cpp:656
Pose3DTests::SetUp
virtual void SetUp()
Definition: CPose3D_unittest.cpp:22
res
GLuint res
Definition: glext.h:6298
x
GLenum GLint x
Definition: glext.h:3516
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:246
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:59
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:342
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:376
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:793
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:757
p
GLfloat GLfloat p
Definition: glext.h:5587
params
GLenum const GLfloat * params
Definition: glext.h:3514
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:1003
Pose3DTests::test_default_values
void test_default_values(const CPose3D &p, const std::string &label)
Definition: CPose3D_unittest.cpp:318
ptc
const double ptc[][6]
Definition: CPose3D_unittest.cpp:638
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:211


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019