Main MRPT website > C++ reference for MRPT 1.5.6
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 
120 
121  TPoint3D l;
122  q1.composePoint(x,y,z, l.x,l.y,l.z, &df_dpoint, &df_dpose);
123 
124  // Numerical approximation:
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 
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:
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 
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 
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 
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  {
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 
388 
389  double hr,hy,hp;
390  q1.sphericalCoordinates(p,hr,hy,hp, &df_dpoint, &df_dpose);
391 
392  // Numerical approximation:
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);
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);
443  pp.getAsQuaternion(q1);
444 
446  q1.normalizationJacobian(df_dpose);
447 
448 
449  // Numerical approximation:
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 
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);
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 
const GLdouble * v
Definition: glew.h:1296
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
void test_sphericalCoords(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
void test_composePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:220
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:75
void test_fromYPRAndBack(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
virtual void TearDown()
void drawGaussian1DVector(VEC &v, const double mean=0, const double std=1)
Fills the given vector with independent, 1D-normally distributed samples.
static void func_normalizeJacob(const CArrayDouble< 4 > &x, const double &dummy, CArrayDouble< 4 > &Y)
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:77
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:191
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:408
void test_copy(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
double z
X,Y,Z coordinates.
GLdouble l
Definition: glew.h:5092
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
static void func_spherical_coords(const CArrayDouble< 7+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:231
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
void test_invComposePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
static void func_compose_point(const CArrayDouble< 7+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
void test_composeAndInvComposePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
#define DEG2RAD
GLfloat GLfloat p
Definition: glew.h:10113
A class used to store a 3D point.
Definition: CPoint3D.h:32
void test_invComposePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
static void func_inv_compose_point(const CArrayDouble< 7+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
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)
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
GLdouble GLdouble z
Definition: glew.h:1464
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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 .
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:78
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
void test_invComposePoint_vs_CPose3D(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
void test_normalizeJacob(double yaw1, double pitch1, double roll1)
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:76
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
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
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
void test_composePoint_vs_CPose3D(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
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::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
Lightweight 3D point.
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 .
void test_unaryInverse(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
void test_composePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
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...
TEST_F(Pose3DQuatTests, FromYPRAndBack)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018