Main MRPT website > C++ reference for MRPT 1.5.7
CPose3DQuat_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/poses/CPose3DQuat.h>
12 #include <mrpt/math/jacobians.h>
13 #include <mrpt/random.h>
14 #include <gtest/gtest.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 using namespace mrpt::utils;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 
23 
24 class Pose3DQuatTests : public ::testing::Test {
25 protected:
26  virtual void SetUp()
27  {
28  }
29 
30  virtual void TearDown() { }
31 
32  void test_compose(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
33  double x2,double y2,double z2, double yaw2,double pitch2,double roll2 )
34  {
35  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
36  const CPose3D p2(x2,y2,z2,yaw2,pitch2,roll2);
37 
38  const CPose3D p1_c_p2 = p1 + p2;
39  const CPose3D p1_i_p2 = p1 - p2;
40 
41 
42  CPose3DQuat q1=CPose3DQuat(p1);
43  CPose3DQuat q2=CPose3DQuat(p2);
44 
45  CPose3DQuat q1_c_q2=q1+q2;
46  CPose3DQuat q1_i_q2=q1-q2;
47 
48  CPose3D p_q1_c_q2 = CPose3D(q1_c_q2);
49  CPose3D p_q1_i_q2 = CPose3D(q1_i_q2);
50 
51  EXPECT_NEAR(0, (p1_c_p2.getAsVectorVal()-p_q1_c_q2.getAsVectorVal()).array().abs().sum(), 1e-5) <<
52  "p1_c_p2: " << p1_c_p2 << endl <<
53  "q1_c_p2: " << p_q1_c_q2 << endl;
54 
55  EXPECT_NEAR(0, (p1_i_p2.getAsVectorVal()-p_q1_i_q2.getAsVectorVal()).array().abs().sum(), 1e-5) <<
56  "p1_i_p2: " << p1_i_p2 << endl <<
57  "q1_i_p2: " << p_q1_i_q2 << endl;
58 
59  // Test + operator: trg new var
60  {
61  CPose3DQuat C = q1;
62  CPose3DQuat A = C + q2;
63  EXPECT_NEAR(0, (A.getAsVectorVal()-q1_c_q2.getAsVectorVal()).array().abs().sum(), 1e-6);
64  }
65  // Test + operator: trg same var
66  {
67  CPose3DQuat A = q1;
68  A = A + q2;
69  EXPECT_NEAR(0, (A.getAsVectorVal()-q1_c_q2.getAsVectorVal()).array().abs().sum(), 1e-6);
70  }
71  // Test =+ operator
72  {
73  CPose3DQuat A = q1;
74  A += q2;
75  EXPECT_NEAR(0, (A.getAsVectorVal()-q1_c_q2.getAsVectorVal()).array().abs().sum(), 1e-6);
76  }
77 
78  }
79 
80  void test_composePoint(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
81  double x,double y,double z)
82  {
83  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
84  const CPose3DQuat q1(p1);
85  const CPoint3D p(x,y,z);
86 
87  CPoint3D p1_plus_p = p1 + p;
88  CPoint3D q1_plus_p = q1 + p;
89 
90  EXPECT_NEAR(0, (p1_plus_p.getAsVectorVal()-q1_plus_p.getAsVectorVal()).array().abs().sum(), 1e-5) <<
91  "p1: " << p1 << endl <<
92  "q1: " << q1 << endl <<
93  "p: " << p << endl <<
94  "p1_plus_p: " << p1_plus_p << endl <<
95  "q1_plus_p: " << q1_plus_p << endl;
96 
97  }
98 
99 
100 
101 
102  static void func_compose_point(const CArrayDouble<7+3> &x, const double &dummy, CArrayDouble<3> &Y)
103  {
104  MRPT_UNUSED_PARAM(dummy);
105  CPose3DQuat q(x[0],x[1],x[2],CQuaternionDouble(x[3],x[4],x[5],x[6]));
106  q.quat().normalize();
107  const CPoint3D p(x[7+0],x[7+1],x[7+2]);
108  const CPoint3D pp = q+p;
109  for (int i=0;i<3;i++) Y[i]=pp[i];
110  }
111 
112  void test_composePointJacob(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
113  double x,double y,double z)
114  {
115  const CPose3DQuat q1( CPose3D(x1,y1,z1,yaw1,pitch1,roll1) );
116  const CPoint3D p(x,y,z);
117 
118  CMatrixFixedNumeric<double,3,3> df_dpoint(UNINITIALIZED_MATRIX);
119  CMatrixFixedNumeric<double,3,7> df_dpose(UNINITIALIZED_MATRIX);
120 
121  TPoint3D l;
122  q1.composePoint(x,y,z, l.x,l.y,l.z, &df_dpoint, &df_dpose);
123 
124  // Numerical approximation:
125  CMatrixFixedNumeric<double,3,3> num_df_dpoint(UNINITIALIZED_MATRIX);
126  CMatrixFixedNumeric<double,3,7> num_df_dpose(UNINITIALIZED_MATRIX);
127  {
128  CArrayDouble<7+3> x_mean;
129  for (int i=0;i<7;i++) x_mean[i]=q1[i];
130  x_mean[7+0]=x;
131  x_mean[7+1]=y;
132  x_mean[7+2]=z;
133 
134  double DUMMY=0;
135  CArrayDouble<7+3> x_incrs;
136  x_incrs.assign(1e-7);
137  CMatrixDouble numJacobs;
138  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose_point,x_incrs, DUMMY, numJacobs );
139 
140  numJacobs.extractMatrix(0,0, num_df_dpose);
141  numJacobs.extractMatrix(0,7, num_df_dpoint);
142  }
143 
144 
145  // Compare:
146  EXPECT_NEAR(0, (df_dpoint-num_df_dpoint).array().abs().sum(), 3e-3 )
147  << "q1: " << q1 << endl
148  << "p: " << p << endl
149  << "Numeric approximation of df_dpoint: " << endl << num_df_dpoint << endl
150  << "Implemented method: " << endl << df_dpoint << endl
151  << "Error: " << endl << df_dpoint-num_df_dpoint << endl;
152 
153  EXPECT_NEAR(0, (df_dpose-num_df_dpose).array().abs().sum(), 3e-3 )
154  << "q1: " << q1 << endl
155  << "p: " << p << endl
156  << "Numeric approximation of df_dpose: " << endl << num_df_dpose << endl
157  << "Implemented method: " << endl << df_dpose << endl
158  << "Error: " << endl << df_dpose-num_df_dpose << endl;
159 
160  }
161 
162  void test_invComposePoint(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
163  double x,double y,double z)
164  {
165  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
166  const CPose3DQuat q1(p1);
167  const CPoint3D p(x,y,z);
168 
169  CPoint3D p_minus_p1 = p - p1;
170  CPoint3D p_minus_q1 = p - q1;
171 
172  CPoint3D p_rec = q1 + p_minus_q1;
173 
174 
175  EXPECT_NEAR(0, (p_minus_p1.getAsVectorVal()-p_minus_q1.getAsVectorVal()).array().abs().sum(), 1e-5) <<
176  "p_minus_p1: " << p_minus_p1 << endl <<
177  "p_minus_q1: " << p_minus_q1 << endl;
178 
179  EXPECT_NEAR(0, (p_rec.getAsVectorVal()-p.getAsVectorVal()).array().abs().sum(), 1e-5) <<
180  "p_rec: " << p_rec << endl <<
181  "p: " << p << endl;
182  }
183 
184  static void func_inv_compose_point(const CArrayDouble<7+3> &x, const double &dummy, CArrayDouble<3> &Y)
185  {
186  MRPT_UNUSED_PARAM(dummy);
187  CPose3DQuat q(x[0],x[1],x[2],CQuaternionDouble(x[3],x[4],x[5],x[6]));
188  q.quat().normalize();
189  const CPoint3D p(x[7+0],x[7+1],x[7+2]);
190  const CPoint3D pp = p-q;
191  Y[0]=pp.x();
192  Y[1]=pp.y();
193  Y[2]=pp.z();
194  }
195 
196  void test_invComposePointJacob(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
197  double x,double y,double z)
198  {
199  const CPose3DQuat q1( CPose3D(x1,y1,z1,yaw1,pitch1,roll1) );
200  const CPoint3D p(x,y,z);
201 
202  CMatrixFixedNumeric<double,3,3> df_dpoint(UNINITIALIZED_MATRIX);
203  CMatrixFixedNumeric<double,3,7> df_dpose(UNINITIALIZED_MATRIX);
204 
205  TPoint3D l;
206  q1.inverseComposePoint(x,y,z, l.x,l.y,l.z, &df_dpoint, &df_dpose);
207 
208  // Also check the returned point, not just the jacobian:
209  TPoint3D theorical;
210  {
211  const double qr = q1.quat().r(); const double qx = q1.quat().x(); const double qy = q1.quat().y(); const double qz = q1.quat().z();
212  const double Ax = x-x1; // ax - x;
213  const double Ay = y-y1; // ay - y;
214  const double Az = z-z1; // az - z;
215  theorical.x = Ax + 2*(Ay)*(qr*qz + qx*qy) - 2*(Az)*(qr*qy - qx*qz) - 2*(square(qy) + square(qz))*(Ax);
216  theorical.y = Ay - 2*(Ax)*(qr*qz - qx*qy) + 2*(Az)*(qr*qx + qy*qz) - 2*(square(qx) + square(qz))*(Ay);
217  theorical.z = Az + 2*(Ax)*(qr*qy + qx*qz) - 2*(Ay)*(qr*qx - qy*qz) - 2*(square(qx) + square(qy))*(Az);
218  }
219  EXPECT_NEAR(theorical.x,l.x, 1e-5);
220  EXPECT_NEAR(theorical.y,l.y, 1e-5);
221  EXPECT_NEAR(theorical.z,l.z, 1e-5);
222 
223  // Numerical approximation:
224  CMatrixFixedNumeric<double,3,3> num_df_dpoint(UNINITIALIZED_MATRIX);
225  CMatrixFixedNumeric<double,3,7> num_df_dpose(UNINITIALIZED_MATRIX);
226  {
227  CArrayDouble<7+3> x_mean;
228  for (int i=0;i<7;i++) x_mean[i]=q1[i];
229  x_mean[7+0]=x;
230  x_mean[7+1]=y;
231  x_mean[7+2]=z;
232 
233  double DUMMY=0;
234  CArrayDouble<7+3> x_incrs;
235  x_incrs.assign(1e-7);
236  CMatrixDouble numJacobs;
237  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_inv_compose_point,x_incrs, DUMMY, numJacobs );
238 
239  numJacobs.extractMatrix(0,0, num_df_dpose);
240  numJacobs.extractMatrix(0,7, num_df_dpoint);
241  }
242 
243  // Compare:
244  EXPECT_NEAR(0, (df_dpoint-num_df_dpoint).array().abs().sum(), 3e-3 )
245  << "q1: " << q1 << endl
246  << "from pose: " << CPose3D(x1,y1,z1,yaw1,pitch1,roll1) << endl
247  << "p: " << p << endl
248  << "local: " << l << endl
249  << "Numeric approximation of df_dpoint: " << endl << num_df_dpoint << endl
250  << "Implemented method: " << endl << df_dpoint << endl
251  << "Error: " << endl << df_dpoint-num_df_dpoint << endl;
252 
253  EXPECT_NEAR(0, (df_dpose-num_df_dpose).array().abs().sum(), 3e-3 )
254  << "q1: " << q1 << endl
255  << "from pose: " << CPose3D(x1,y1,z1,yaw1,pitch1,roll1) << endl
256  << "p: " << p << endl
257  << "local: " << l << endl
258  << "Numeric approximation of df_dpose: " << endl << num_df_dpose << endl
259  << "Implemented method: " << endl << df_dpose << endl
260  << "Error: " << endl << df_dpose-num_df_dpose << endl;
261  }
262 
263 
264  void test_fromYPRAndBack(double x1,double y1,double z1, double yaw1,double pitch1,double roll1)
265  {
266  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
267  const CPose3DQuat q1(p1);
268  const CPose3D p1r = CPose3D(q1);
269 
270  EXPECT_NEAR(0, (p1.getHomogeneousMatrixVal()-q1.getHomogeneousMatrixVal()).array().abs().sum(), 1e-5) <<
271  "p1.getHomogeneousMatrixVal():\n" << p1.getHomogeneousMatrixVal() << endl <<
272  "q1.getHomogeneousMatrixVal():\n" << q1.getHomogeneousMatrixVal() << endl;
273 
274  EXPECT_NEAR(0, (p1.getAsVectorVal()-p1r.getAsVectorVal()).array().abs().sum(), 1e-5) <<
275  "p1: " << p1 << endl <<
276  "q1: " << q1 << endl <<
277  "p1r: " << p1r << endl;
278  }
279 
280  void test_unaryInverse(double x1,double y1,double z1, double yaw1,double pitch1,double roll1)
281  {
282  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
283  const CPose3D p1_inv = -p1;
284 
285  const CPose3DQuat q1(p1);
286  const CPose3DQuat q1_inv = -q1;
287 
288  EXPECT_NEAR(0, (p1_inv.getHomogeneousMatrixVal()-q1_inv.getHomogeneousMatrixVal()).array().abs().sum(), 1e-5) <<
289  "p1_inv.getHomogeneousMatrixVal():\n" << p1_inv.getHomogeneousMatrixVal() << endl <<
290  "q1_inv.getHomogeneousMatrixVal():\n" << q1_inv.getHomogeneousMatrixVal() << endl;
291 
292  }
293 
294  void test_copy(double x1,double y1,double z1, double yaw1,double pitch1,double roll1)
295  {
296  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
297 
298  const CPose3DQuat q1(p1);
299  const CPose3DQuat q2 = q1;
300 
301  EXPECT_NEAR(0, (q1.getHomogeneousMatrixVal()-q2.getHomogeneousMatrixVal()).array().abs().sum(), 1e-5) <<
302  "q1.getHomogeneousMatrixVal():\n" << q1.getHomogeneousMatrixVal() << endl <<
303  "q2.getHomogeneousMatrixVal():\n" << q2.getHomogeneousMatrixVal() << endl;
304 
305  }
306 
307  void test_composeAndInvComposePoint(
308  double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
309  double x, double y, double z)
310  {
311  const CPose3DQuat q1(CPose3D(x1,y1,z1,yaw1,pitch1,roll1));
312  TPoint3D pp(x,y,z), aux;
313  q1.composePoint( x,y,z, pp.x,pp.y,pp.z);
314  q1.inverseComposePoint( pp.x,pp.y,pp.z, aux.x, aux.y, aux.z );
315 
316  EXPECT_NEAR(x,aux.x, 1e-7);
317  EXPECT_NEAR(y,aux.y, 1e-7);
318  EXPECT_NEAR(z,aux.z, 1e-7);
319  }
320 
321  void test_composePoint_vs_CPose3D(
322  double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
323  double x, double y, double z)
324  {
325  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
326  const CPose3DQuat q1(p1);
327  TPoint3D pt1,pt2;
328  p1.composePoint( x,y,z, pt1.x,pt1.y,pt1.z);
329  q1.composePoint( x,y,z, pt2.x,pt2.y,pt2.z);
330 
331  EXPECT_NEAR(pt1.x,pt2.x, 1e-7);
332  EXPECT_NEAR(pt1.y,pt2.y, 1e-7);
333  EXPECT_NEAR(pt1.z,pt2.z, 1e-7);
334  }
335 
336  void test_invComposePoint_vs_CPose3D(
337  double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
338  double x, double y, double z)
339  {
340  const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
341  const CPose3DQuat q1(p1);
342  TPoint3D pt1,pt2;
343  p1.inverseComposePoint( x,y,z, pt1.x,pt1.y,pt1.z);
344  q1.inverseComposePoint( x,y,z, pt2.x,pt2.y,pt2.z);
345 
346  EXPECT_NEAR(pt1.x,pt2.x, 1e-7);
347  EXPECT_NEAR(pt1.y,pt2.y, 1e-7);
348  EXPECT_NEAR(pt1.z,pt2.z, 1e-7);
349 
350  {
351  CPose3DQuat q = mrpt::poses::CPose3DQuat(p1);
352 
353  float gx=x,gy=y,gz=z;
354 
355  double dist,yaw,pitch;
356  p1.sphericalCoordinates(TPoint3D(gx,gy,gz), dist,yaw,pitch);
357 
358  double lx,ly,lz;
359  p1.inverseComposePoint(gx,gy,gz, lx,ly,lz);
360 
361  double lx2,ly2,lz2;
362  q.inverseComposePoint(gx,gy,gz, lx2,ly2,lz2);
363 
364  EXPECT_NEAR(lx,lx2, 1e-7);
365  EXPECT_NEAR(ly,ly2, 1e-7);
366  EXPECT_NEAR(lz,lz2, 1e-7);
367  }
368 
369  }
370 
371  static void func_spherical_coords(const CArrayDouble<7+3> &x, const double &dummy, CArrayDouble<3> &Y)
372  {
373  MRPT_UNUSED_PARAM(dummy);
374  CPose3DQuat q(x[0],x[1],x[2],CQuaternionDouble(x[3],x[4],x[5],x[6]));
375  q.quat().normalize();
376  const TPoint3D p(x[7+0],x[7+1],x[7+2]);
377  q.sphericalCoordinates(p,Y[0],Y[1],Y[2]);
378  }
379 
380  void test_sphericalCoords(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
381  double x,double y,double z)
382  {
383  const CPose3DQuat q1( CPose3D(x1,y1,z1,yaw1,pitch1,roll1) );
384  const TPoint3D p(x,y,z);
385 
386  CMatrixFixedNumeric<double,3,3> df_dpoint(UNINITIALIZED_MATRIX);
387  CMatrixFixedNumeric<double,3,7> df_dpose(UNINITIALIZED_MATRIX);
388 
389  double hr,hy,hp;
390  q1.sphericalCoordinates(p,hr,hy,hp, &df_dpoint, &df_dpose);
391 
392  // Numerical approximation:
393  CMatrixFixedNumeric<double,3,3> num_df_dpoint(UNINITIALIZED_MATRIX);
394  CMatrixFixedNumeric<double,3,7> num_df_dpose(UNINITIALIZED_MATRIX);
395  {
396  CArrayDouble<7+3> x_mean;
397  for (int i=0;i<7;i++) x_mean[i]=q1[i];
398  x_mean[7+0]=x;
399  x_mean[7+1]=y;
400  x_mean[7+2]=z;
401 
402  double DUMMY=0;
403  CArrayDouble<7+3> x_incrs;
404  x_incrs.assign(1e-7);
405  CMatrixDouble numJacobs;
406  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_spherical_coords,x_incrs, DUMMY, numJacobs );
407 
408  numJacobs.extractMatrix(0,0, num_df_dpose);
409  numJacobs.extractMatrix(0,7, num_df_dpoint);
410  }
411 
412 
413  // Compare:
414  EXPECT_NEAR(0, (df_dpoint-num_df_dpoint).array().abs().sum(), 3e-3 )
415  << "q1: " << q1 << endl
416  << "p: " << p << endl
417  << "Numeric approximation of df_dpoint: " << endl << num_df_dpoint << endl
418  << "Implemented method: " << endl << df_dpoint << endl
419  << "Error: " << endl << df_dpoint-num_df_dpoint << endl;
420 
421  EXPECT_NEAR(0, (df_dpose-num_df_dpose).array().abs().sum(), 3e-3 )
422  << "q1: " << q1 << endl
423  << "p: " << p << endl
424  << "Numeric approximation of df_dpose: " << endl << num_df_dpose << endl
425  << "Implemented method: " << endl << df_dpose << endl
426  << "Error: " << endl << df_dpose-num_df_dpose << endl;
427 
428  }
429 
430  static void func_normalizeJacob(const CArrayDouble<4> &x, const double &dummy, CArrayDouble<4> &Y)
431  {
432  MRPT_UNUSED_PARAM(dummy);
433  CQuaternionDouble q;
434  for (int i=0;i<4;i++) q[i]=x[i];
435  q.normalize();
436  for (int i=0;i<4;i++) Y[i]=q[i];
437  }
438 
439  void test_normalizeJacob(double yaw1,double pitch1,double roll1)
440  {
441  const CPose3D pp(0,0,0,yaw1,pitch1,roll1);
442  CQuaternionDouble q1;
443  pp.getAsQuaternion(q1);
444 
445  CMatrixFixedNumeric<double,4,4> df_dpose(UNINITIALIZED_MATRIX);
446  q1.normalizationJacobian(df_dpose);
447 
448 
449  // Numerical approximation:
450  CMatrixFixedNumeric<double,4,4> num_df_dpose(UNINITIALIZED_MATRIX);
451  {
452  CArrayDouble<4> x_mean;
453  for (int i=0;i<4;i++) x_mean[i]=q1[i];
454 
455  double DUMMY=0;
456  CArrayDouble<4> x_incrs;
457  x_incrs.assign(1e-5);
458  CMatrixDouble numJacobs;
459  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_normalizeJacob,x_incrs, DUMMY, numJacobs );
460 
461  numJacobs.extractMatrix(0,0, num_df_dpose);
462  }
463 
464 
465  // Compare:
466  EXPECT_NEAR(0, (df_dpose-num_df_dpose).array().abs().sum(), 3e-3 )
467  << "q1: " << q1 << endl
468  << "Numeric approximation of df_dpose: " << endl << num_df_dpose << endl
469  << "Implemented method: " << endl << df_dpose << endl
470  << "Error: " << endl << df_dpose-num_df_dpose << endl;
471  }
472 
473 };
474 
475 
476 TEST_F(Pose3DQuatTests,FromYPRAndBack)
477 {
478  test_fromYPRAndBack(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0));
479  test_fromYPRAndBack(1.0,2.0,3.0, DEG2RAD(90),DEG2RAD(0),DEG2RAD(0));
480  test_fromYPRAndBack(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60));
481  test_fromYPRAndBack(1.0,2.0,3.0, DEG2RAD(179),DEG2RAD(0),DEG2RAD(60));
482  test_fromYPRAndBack(1.0,2.0,3.0, DEG2RAD(-179),DEG2RAD(0),DEG2RAD(60));
483  test_fromYPRAndBack(1.0,2.0,3.0, DEG2RAD(30),DEG2RAD( 89),DEG2RAD(0));
484  test_fromYPRAndBack(1.0,2.0,3.0, DEG2RAD(30),DEG2RAD(-89),DEG2RAD(0));
485 }
486 
487 TEST_F(Pose3DQuatTests,UnaryInverse)
488 {
489  test_unaryInverse(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0));
490  test_unaryInverse(1.0,2.0,3.0, DEG2RAD(90),DEG2RAD(0),DEG2RAD(0));
491  test_unaryInverse(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60));
492  test_unaryInverse(1.0,2.0,3.0, DEG2RAD(179),DEG2RAD(0),DEG2RAD(60));
493  test_unaryInverse(1.0,2.0,3.0, DEG2RAD(-179),DEG2RAD(0),DEG2RAD(60));
494  test_unaryInverse(1.0,2.0,3.0, DEG2RAD(30),DEG2RAD( 89),DEG2RAD(0));
495  test_unaryInverse(1.0,2.0,3.0, DEG2RAD(30),DEG2RAD(-89),DEG2RAD(0));
496 }
497 
498 
499 TEST_F(Pose3DQuatTests,CopyOperator)
500 {
501  test_copy(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0));
502  test_copy(1.0,2.0,3.0, DEG2RAD(90),DEG2RAD(0),DEG2RAD(0));
503  test_copy(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60));
504  test_copy(1.0,2.0,3.0, DEG2RAD(30),DEG2RAD(-89),DEG2RAD(0));
505 }
506 
507 
508 TEST_F(Pose3DQuatTests,Compose)
509 {
510  test_compose(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60),
511  2.0,-5.0,8.0, DEG2RAD(40),DEG2RAD(-5),DEG2RAD(25));
512 
513  test_compose(25.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(90),DEG2RAD(0),
514  -10.0,4.0,-8.0, DEG2RAD(20),DEG2RAD(9),DEG2RAD(0));
515 }
516 
517 TEST_F(Pose3DQuatTests,ComposeWithPoint)
518 {
519  test_composePoint(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
520  test_composePoint(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
521  test_composePoint(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 10,11,12 );
522  test_composePoint(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 10,11,12 );
523  test_composePoint(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60), 10.0, 20.0, 30.0 );
524  test_composePoint(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(-50),DEG2RAD(-40), -5.0, -15.0, 8.0 );
525 }
526 
527 TEST_F(Pose3DQuatTests,ComposeWithPointJacob)
528 {
529  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
530  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
531  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 10,11,12 );
532  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 10,11,12 );
533  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60), 10.0, 20.0, 30.0 );
534  test_composePointJacob(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(-50),DEG2RAD(-40), -5.0, -15.0, 8.0 );
535 }
536 
537 TEST_F(Pose3DQuatTests,InvComposeWithPoint)
538 {
539  test_invComposePoint(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
540  test_invComposePoint(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
541  test_invComposePoint(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 10,11,12 );
542  test_invComposePoint(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 10,11,12 );
543  test_invComposePoint(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60), 10.0, 20.0, 30.0 );
544  test_invComposePoint(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(-50),DEG2RAD(-40), -5.0, -15.0, 8.0 );
545 }
546 
547 TEST_F(Pose3DQuatTests,InvComposeWithPointJacob)
548 {
549  test_invComposePointJacob(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0,0,0 );
550  test_invComposePointJacob(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 1,2,3 );
551  test_invComposePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0,0,0);
552  test_invComposePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
553  test_invComposePointJacob(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
554  test_invComposePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 10,11,12 );
555  test_invComposePointJacob(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 10,11,12 );
556  test_invComposePointJacob(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60), 10.0, 20.0, 30.0 );
557  test_invComposePointJacob(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(-50),DEG2RAD(-40), -5.0, -15.0, 8.0 );
558 }
559 
560 TEST_F(Pose3DQuatTests,ComposeInvComposePoint)
561 {
562  test_composeAndInvComposePoint(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
563  test_composeAndInvComposePoint(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
564  test_composeAndInvComposePoint(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 10,11,12 );
565  test_composeAndInvComposePoint(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 10,11,12 );
566  test_composeAndInvComposePoint(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60), 10.0, 20.0, 30.0 );
567  test_composeAndInvComposePoint(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(-50),DEG2RAD(-40), -5.0, -15.0, 8.0 );
568 }
569 
570 TEST_F(Pose3DQuatTests,ComposePoint_vs_CPose3D)
571 {
572  test_composePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
573  test_composePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
574  test_composePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 10,11,12 );
575  test_composePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 10,11,12 );
576  test_composePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60), 10.0, 20.0, 30.0 );
577  test_composePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(-50),DEG2RAD(-40), -5.0, -15.0, 8.0 );
578 }
579 
580 TEST_F(Pose3DQuatTests,InvComposePoint_vs_CPose3D)
581 {
582  test_invComposePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
583  test_invComposePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
584  test_invComposePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 10,11,12 );
585  test_invComposePoint_vs_CPose3D(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 10,11,12 );
586 
587  for (size_t i=0;i<10;i++)
588  {
589  std::vector<double> v(9);
590  mrpt::random::randomGenerator.drawGaussian1DVector(v,0,1);
591  test_invComposePoint_vs_CPose3D(v[0],v[1],v[2],v[3],v[4],v[5], v[6],v[7],v[8]);
592  }
593 }
594 
595 TEST_F(Pose3DQuatTests,SphericalCoordsJacobian)
596 {
597  test_sphericalCoords(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
598  test_sphericalCoords(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 10,11,12 );
599  test_sphericalCoords(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 10,11,12 );
600  test_sphericalCoords(1.0,2.0,3.0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 10,11,12 );
601  test_sphericalCoords(1.0,2.0,3.0, DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60), 10.0, 20.0, 30.0 );
602  test_sphericalCoords(1.0,2.0,3.0, DEG2RAD(10),DEG2RAD(-50),DEG2RAD(-40), -5.0, -15.0, 8.0 );
603 }
604 
605 TEST_F(Pose3DQuatTests,NormalizationJacobian)
606 {
607  test_normalizeJacob(DEG2RAD(0),DEG2RAD(0),DEG2RAD(0));
608  test_normalizeJacob(DEG2RAD(10),DEG2RAD(0),DEG2RAD(0));
609  test_normalizeJacob(DEG2RAD(0),DEG2RAD(10),DEG2RAD(0));
610  test_normalizeJacob(DEG2RAD(0),DEG2RAD(0),DEG2RAD(10));
611  test_normalizeJacob(DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60));
612  test_normalizeJacob(DEG2RAD(10),DEG2RAD(-50),DEG2RAD(-40));
613 }
614 
TEST_F
TEST_F(Pose3DQuatTests, FromYPRAndBack)
Definition: CPose3DQuat_unittest.cpp:476
DEG2RAD
#define DEG2RAD
Definition: bits.h:99
Pose3DQuatTests::test_unaryInverse
void test_unaryInverse(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:280
Pose3DQuatTests::test_invComposePointJacob
void test_invComposePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:196
Pose3DQuatTests::func_normalizeJacob
static void func_normalizeJacob(const CArrayDouble< 4 > &x, const double &dummy, CArrayDouble< 4 > &Y)
Definition: CPose3DQuat_unittest.cpp:430
Pose3DQuatTests::test_composePoint
void test_composePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:80
Pose3DQuatTests::func_inv_compose_point
static void func_inv_compose_point(const CArrayDouble< 7+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:184
Pose3DQuatTests::test_composePointJacob
void test_composePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:112
Pose3DQuatTests::TearDown
virtual void TearDown()
Definition: CPose3DQuat_unittest.cpp:30
Pose3DQuatTests::test_composeAndInvComposePoint
void test_composeAndInvComposePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:307
Pose3DQuatTests::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: CPose3DQuat_unittest.cpp:32
Pose3DQuatTests::test_composePoint_vs_CPose3D
void test_composePoint_vs_CPose3D(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:321
Pose3DQuatTests::func_compose_point
static void func_compose_point(const CArrayDouble< 7+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:102
Pose3DQuatTests::test_invComposePoint_vs_CPose3D
void test_invComposePoint_vs_CPose3D(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:336
Pose3DQuatTests::test_fromYPRAndBack
void test_fromYPRAndBack(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:264
Pose3DQuatTests::test_invComposePoint
void test_invComposePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:162
Pose3DQuatTests::test_copy
void test_copy(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:294
Pose3DQuatTests::test_sphericalCoords
void test_sphericalCoords(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:380
Pose3DQuatTests::test_normalizeJacob
void test_normalizeJacob(double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:439
Pose3DQuatTests::func_spherical_coords
static void func_spherical_coords(const CArrayDouble< 7+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:371
mrpt::math::CArrayDouble
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:75
mrpt::math::CQuaternion
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
Definition: CQuaternion.h:44
mrpt::math::CQuaternion::z
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:79
mrpt::math::CQuaternion::x
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:77
mrpt::math::CQuaternion::r
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:76
mrpt::math::CQuaternion::y
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:78
mrpt::math::CQuaternion::normalizationJacobian
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:232
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:33
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
mrpt::poses::CPose3D::sphericalCoordinates
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
Definition: CPose3D.cpp:365
mrpt::poses::CPose3D::getHomogeneousMatrixVal
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
mrpt::poses::CPose3D::getAsQuaternion
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
Definition: CPose3D.cpp:553
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::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
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:42
mrpt::poses::CPose3DQuat::sphericalCoordinates
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacob_dryp_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacob_dryp_dpose=NULL) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
Definition: CPose3DQuat.cpp:332
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
mrpt::poses::CPose3DQuat::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, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point L such as .
Definition: CPose3DQuat.cpp:181
mrpt::poses::CPose3DQuat::composePoint
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point G such as .
Definition: CPose3DQuat.cpp:105
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
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
mrpt::poses::CPoseOrPoint::getHomogeneousMatrixVal
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:191
mrpt::random::CRandomGenerator::drawGaussian1DVector
void drawGaussian1DVector(VEC &v, const double mean=0, const double std=1)
Fills the given vector with independent, 1D-normally distributed samples.
Definition: RandomGenerators.h:166
v
const GLdouble * v
Definition: glext.h:3603
y
GLenum GLint GLint y
Definition: glext.h:3516
x
GLenum GLint x
Definition: glext.h:3516
p
GLfloat GLfloat p
Definition: glext.h:5587
z
GLdouble GLdouble z
Definition: glext.h:3734
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:307
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
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:20
mrpt::math::UNINITIALIZED_MATRIX
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:74
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:167
mrpt::math::CQuaternionDouble
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:408
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:18
mrpt::random::randomGenerator
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:23
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: zip.h:16
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:246
mrpt::math::TPoint3D::z
double z
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:248


Page generated by Doxygen 1.9.1 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at mar 26 may 2026 13:12:03 CEST