MRPT  1.9.9
CPose3DQuat_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
12 #include <mrpt/math/num_jacobian.h>
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPose3DQuat.h>
15 #include <mrpt/random.h>
16 #include <Eigen/Dense>
17 
18 using namespace mrpt;
19 using namespace mrpt::poses;
20 
21 using namespace mrpt::math;
22 using namespace std;
23 
24 template class mrpt::CTraitsTest<CPose3DQuat>;
25 
26 class Pose3DQuatTests : public ::testing::Test
27 {
28  protected:
29  void SetUp() override {}
30  void TearDown() override {}
32  double x1, double y1, double z1, double yaw1, double pitch1,
33  double roll1, double x2, double y2, double z2, double yaw2,
34  double pitch2, double roll2)
35  {
36  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
37  const CPose3D p2(x2, y2, z2, yaw2, pitch2, roll2);
38 
39  const CPose3D p1_c_p2 = p1 + p2;
40  const CPose3D p1_i_p2 = p1 - p2;
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 
52  0,
53  (p1_c_p2.asVectorVal() - p_q1_c_q2.asVectorVal())
54  .array()
55  .abs()
56  .sum(),
57  1e-5)
58  << "p1_c_p2: " << p1_c_p2 << endl
59  << "q1_c_p2: " << p_q1_c_q2 << endl;
60 
62  0,
63  (p1_i_p2.asVectorVal() - p_q1_i_q2.asVectorVal())
64  .array()
65  .abs()
66  .sum(),
67  1e-5)
68  << "p1_i_p2: " << p1_i_p2 << endl
69  << "q1_i_p2: " << p_q1_i_q2 << endl;
70 
71  // Test + operator: trg new var
72  {
73  CPose3DQuat C = q1;
74  CPose3DQuat A = C + q2;
76  0,
77  (A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
78  1e-6);
79  }
80  // Test + operator: trg same var
81  {
82  CPose3DQuat A = q1;
83  A = A + q2;
85  0,
86  (A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
87  1e-6);
88  }
89  // Test =+ operator
90  {
91  CPose3DQuat A = q1;
92  A += q2;
94  0,
95  (A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
96  1e-6);
97  }
98  }
99 
101  double x1, double y1, double z1, double yaw1, double pitch1,
102  double roll1, double x, double y, double z)
103  {
104  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
105  const CPose3DQuat q1(p1);
106  const CPoint3D p(x, y, z);
107 
108  CPoint3D p1_plus_p = p1 + p;
109  CPoint3D q1_plus_p = q1 + p;
110 
111  EXPECT_NEAR(
112  0,
113  (p1_plus_p.asVectorVal() - q1_plus_p.asVectorVal())
114  .array()
115  .abs()
116  .sum(),
117  1e-5)
118  << "p1: " << p1 << endl
119  << "q1: " << q1 << endl
120  << "p: " << p << endl
121  << "p1_plus_p: " << p1_plus_p << endl
122  << "q1_plus_p: " << q1_plus_p << endl;
123  }
124 
125  static void func_compose_point(
126  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
128  {
129  MRPT_UNUSED_PARAM(dummy);
130  CPose3DQuat q(
131  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
132  q.quat().normalize();
133  const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
134  const CPoint3D pp = q + p;
135  for (int i = 0; i < 3; i++) Y[i] = pp[i];
136  }
137 
139  double x1, double y1, double z1, double yaw1, double pitch1,
140  double roll1, double x, double y, double z)
141  {
142  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
143  const CPoint3D p(x, y, z);
144 
147 
148  TPoint3D l;
149  q1.composePoint(x, y, z, l.x, l.y, l.z, &df_dpoint, &df_dpose);
150 
151  // Numerical approximation:
154  {
156  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
157  x_mean[7 + 0] = x;
158  x_mean[7 + 1] = y;
159  x_mean[7 + 2] = z;
160 
161  double DUMMY = 0;
163  x_incrs.fill(1e-7);
164  CMatrixDouble numJacobs;
166  x_mean,
167  std::function<void(
168  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
169  CVectorFixedDouble<3>& Y)>(&func_compose_point),
170  x_incrs, DUMMY, numJacobs);
171 
172  num_df_dpose = numJacobs.asEigen().block<3, 7>(0, 0);
173  num_df_dpoint = numJacobs.asEigen().block<3, 3>(0, 7);
174  }
175 
176  // Compare:
177  EXPECT_NEAR(
178  0,
179  (df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
180  3e-3)
181  << "q1: " << q1 << endl
182  << "p: " << p << endl
183  << "Numeric approximation of df_dpoint: " << endl
184  << num_df_dpoint.asEigen() << endl
185  << "Implemented method: " << endl
186  << df_dpoint << endl
187  << "Error: " << endl
188  << df_dpoint.asEigen() - num_df_dpoint.asEigen() << endl;
189 
190  EXPECT_NEAR(
191  0,
192  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
193  3e-3)
194  << "q1: " << q1 << endl
195  << "p: " << p << endl
196  << "Numeric approximation of df_dpose: " << endl
197  << num_df_dpose.asEigen() << endl
198  << "Implemented method: " << endl
199  << df_dpose.asEigen() << endl
200  << "Error: " << endl
201  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
202  }
203 
205  double x1, double y1, double z1, double yaw1, double pitch1,
206  double roll1, double x, double y, double z)
207  {
208  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
209  const CPose3DQuat q1(p1);
210  const CPoint3D p(x, y, z);
211 
212  CPoint3D p_minus_p1 = p - p1;
213  CPoint3D p_minus_q1 = p - q1;
214 
215  CPoint3D p_rec = q1 + p_minus_q1;
216 
217  EXPECT_NEAR(
218  0,
219  (p_minus_p1.asVectorVal() - p_minus_q1.asVectorVal())
220  .array()
221  .abs()
222  .sum(),
223  1e-5)
224  << "p_minus_p1: " << p_minus_p1 << endl
225  << "p_minus_q1: " << p_minus_q1 << endl;
226 
227  EXPECT_NEAR(
228  0, (p_rec.asVectorVal() - p.asVectorVal()).array().abs().sum(),
229  1e-5)
230  << "p_rec: " << p_rec << endl
231  << "p: " << p << endl;
232  }
233 
235  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
237  {
238  MRPT_UNUSED_PARAM(dummy);
239  CPose3DQuat q(
240  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
241  q.quat().normalize();
242  const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
243  const CPoint3D pp = p - q;
244  Y[0] = pp.x();
245  Y[1] = pp.y();
246  Y[2] = pp.z();
247  }
248 
250  double x1, double y1, double z1, double yaw1, double pitch1,
251  double roll1, double x, double y, double z)
252  {
253  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
254  const CPoint3D p(x, y, z);
255 
258 
259  TPoint3D l;
260  q1.inverseComposePoint(x, y, z, l.x, l.y, l.z, &df_dpoint, &df_dpose);
261 
262  // Also check the returned point, not just the jacobian:
263  TPoint3D theorical;
264  {
265  const double qr = q1.quat().r();
266  const double qx = q1.quat().x();
267  const double qy = q1.quat().y();
268  const double qz = q1.quat().z();
269  const double Ax = x - x1; // ax - x;
270  const double Ay = y - y1; // ay - y;
271  const double Az = z - z1; // az - z;
272  theorical.x = Ax + 2 * (Ay) * (qr * qz + qx * qy) -
273  2 * (Az) * (qr * qy - qx * qz) -
274  2 * (square(qy) + square(qz)) * (Ax);
275  theorical.y = Ay - 2 * (Ax) * (qr * qz - qx * qy) +
276  2 * (Az) * (qr * qx + qy * qz) -
277  2 * (square(qx) + square(qz)) * (Ay);
278  theorical.z = Az + 2 * (Ax) * (qr * qy + qx * qz) -
279  2 * (Ay) * (qr * qx - qy * qz) -
280  2 * (square(qx) + square(qy)) * (Az);
281  }
282  EXPECT_NEAR(theorical.x, l.x, 1e-5);
283  EXPECT_NEAR(theorical.y, l.y, 1e-5);
284  EXPECT_NEAR(theorical.z, l.z, 1e-5);
285 
286  // Numerical approximation:
289  {
291  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
292  x_mean[7 + 0] = x;
293  x_mean[7 + 1] = y;
294  x_mean[7 + 2] = z;
295 
296  double DUMMY = 0;
298  x_incrs.fill(1e-7);
299  CMatrixDouble numJacobs;
301  x_mean,
302  std::function<void(
303  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
304  CVectorFixedDouble<3>& Y)>(&func_inv_compose_point),
305  x_incrs, DUMMY, numJacobs);
306 
307  num_df_dpose = numJacobs.block<3, 7>(0, 0);
308  num_df_dpoint = numJacobs.block<3, 3>(0, 7);
309  }
310 
311  // Compare:
312  EXPECT_NEAR(
313  0,
314  (df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
315  3e-3)
316  << "q1: " << q1 << endl
317  << "from pose: " << CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
318  << "p: " << p << endl
319  << "local: " << l << endl
320  << "Numeric approximation of df_dpoint: " << endl
321  << num_df_dpoint.asEigen() << endl
322  << "Implemented method: " << endl
323  << df_dpoint.asEigen() << endl
324  << "Error: " << endl
325  << df_dpoint - num_df_dpoint << endl;
326 
327  EXPECT_NEAR(
328  0,
329  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
330  3e-3)
331  << "q1: " << q1 << endl
332  << "from pose: " << CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
333  << "p: " << p << endl
334  << "local: " << l << endl
335  << "Numeric approximation of df_dpose: " << endl
336  << num_df_dpose.asEigen() << endl
337  << "Implemented method: " << endl
338  << df_dpose.asEigen() << endl
339  << "Error: " << endl
340  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
341  }
342 
344  double x1, double y1, double z1, double yaw1, double pitch1,
345  double roll1)
346  {
347  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
348  const CPose3DQuat q1(p1);
349  const CPose3D p1r = CPose3D(q1);
350 
351  EXPECT_NEAR(
352  0,
355  .array()
356  .abs()
357  .sum(),
358  1e-5)
359  << "p1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
361  << "q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
362  << q1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
363 
364  EXPECT_NEAR(
365  0, (p1.asVectorVal() - p1r.asVectorVal()).array().abs().sum(), 1e-5)
366  << "p1: " << p1 << endl
367  << "q1: " << q1 << endl
368  << "p1r: " << p1r << endl;
369  }
370 
372  double x1, double y1, double z1, double yaw1, double pitch1,
373  double roll1)
374  {
375  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
376  const CPose3D p1_inv = -p1;
377 
378  const CPose3DQuat q1(p1);
379  const CPose3DQuat q1_inv = -q1;
380 
381  EXPECT_NEAR(
382  0,
385  .array()
386  .abs()
387  .sum(),
388  1e-5)
389  << "p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
390  << p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
391  << "q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
392  << q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
393  }
394 
395  void test_copy(
396  double x1, double y1, double z1, double yaw1, double pitch1,
397  double roll1)
398  {
399  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
400 
401  const CPose3DQuat q1(p1);
402  const CPose3DQuat q2 = q1;
403 
404  EXPECT_NEAR(
405  0,
408  .array()
409  .abs()
410  .sum(),
411  1e-5)
412  << "q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
414  << "q2.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
415  << q2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
416  }
417 
419  double x1, double y1, double z1, double yaw1, double pitch1,
420  double roll1, double x, double y, double z)
421  {
422  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
423  TPoint3D pp(x, y, z), aux;
424  q1.composePoint(x, y, z, pp.x, pp.y, pp.z);
425  q1.inverseComposePoint(pp.x, pp.y, pp.z, aux.x, aux.y, aux.z);
426 
427  EXPECT_NEAR(x, aux.x, 1e-7);
428  EXPECT_NEAR(y, aux.y, 1e-7);
429  EXPECT_NEAR(z, aux.z, 1e-7);
430  }
431 
433  double x1, double y1, double z1, double yaw1, double pitch1,
434  double roll1, double x, double y, double z)
435  {
436  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
437  const CPose3DQuat q1(p1);
438  TPoint3D pt1, pt2;
439  p1.composePoint(x, y, z, pt1.x, pt1.y, pt1.z);
440  q1.composePoint(x, y, z, pt2.x, pt2.y, pt2.z);
441 
442  EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
443  EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
444  EXPECT_NEAR(pt1.z, pt2.z, 1e-7);
445  }
446 
448  double x1, double y1, double z1, double yaw1, double pitch1,
449  double roll1, double x, double y, double z)
450  {
451  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
452  const CPose3DQuat q1(p1);
453  TPoint3D pt1, pt2;
454  p1.inverseComposePoint(x, y, z, pt1.x, pt1.y, pt1.z);
455  q1.inverseComposePoint(x, y, z, pt2.x, pt2.y, pt2.z);
456 
457  EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
458  EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
459  EXPECT_NEAR(pt1.z, pt2.z, 1e-7);
460 
461  {
463 
464  float gx = x, gy = y, gz = z;
465 
466  double dist, yaw, pitch;
467  p1.sphericalCoordinates(TPoint3D(gx, gy, gz), dist, yaw, pitch);
468 
469  double lx, ly, lz;
470  p1.inverseComposePoint(gx, gy, gz, lx, ly, lz);
471 
472  double lx2, ly2, lz2;
473  q.inverseComposePoint(gx, gy, gz, lx2, ly2, lz2);
474 
475  EXPECT_NEAR(lx, lx2, 1e-7);
476  EXPECT_NEAR(ly, ly2, 1e-7);
477  EXPECT_NEAR(lz, lz2, 1e-7);
478  }
479  }
480 
482  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
484  {
485  MRPT_UNUSED_PARAM(dummy);
486  CPose3DQuat q(
487  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
488  q.quat().normalize();
489  const TPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
490  q.sphericalCoordinates(p, Y[0], Y[1], Y[2]);
491  }
492 
494  double x1, double y1, double z1, double yaw1, double pitch1,
495  double roll1, double x, double y, double z)
496  {
497  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
498  const TPoint3D p(x, y, z);
499 
502 
503  double hr, hy, hp;
504  q1.sphericalCoordinates(p, hr, hy, hp, &df_dpoint, &df_dpose);
505 
506  // Numerical approximation:
509  {
511  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
512  x_mean[7 + 0] = x;
513  x_mean[7 + 1] = y;
514  x_mean[7 + 2] = z;
515 
516  double DUMMY = 0;
518  x_incrs.fill(1e-7);
519  CMatrixDouble numJacobs;
521  x_mean,
522  std::function<void(
523  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
524  CVectorFixedDouble<3>& Y)>(&func_spherical_coords),
525  x_incrs, DUMMY, numJacobs);
526 
527  num_df_dpose = numJacobs.block<3, 7>(0, 0);
528  num_df_dpoint = numJacobs.block<3, 3>(0, 7);
529  }
530 
531  // Compare:
532  EXPECT_NEAR(
533  0,
534  (df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
535  3e-3)
536  << "q1: " << q1 << endl
537  << "p: " << p << endl
538  << "Numeric approximation of df_dpoint: " << endl
539  << num_df_dpoint.asEigen() << endl
540  << "Implemented method: " << endl
541  << df_dpoint.asEigen() << endl
542  << "Error: " << endl
543  << df_dpoint.asEigen() - num_df_dpoint.asEigen() << endl;
544 
545  EXPECT_NEAR(
546  0,
547  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
548  3e-3)
549  << "q1: " << q1 << endl
550  << "p: " << p << endl
551  << "Numeric approximation of df_dpose: " << endl
552  << num_df_dpose.asEigen() << endl
553  << "Implemented method: " << endl
554  << df_dpose.asEigen() << endl
555  << "Error: " << endl
556  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
557  }
558 
559  static void func_normalizeJacob(
560  const CVectorFixedDouble<4>& x, const double& dummy,
562  {
563  MRPT_UNUSED_PARAM(dummy);
565  for (int i = 0; i < 4; i++) q[i] = x[i];
566  q.normalize();
567  for (int i = 0; i < 4; i++) Y[i] = q[i];
568  }
569 
570  void test_normalizeJacob(double yaw1, double pitch1, double roll1)
571  {
572  const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
574  pp.getAsQuaternion(q1);
575 
577  q1.normalizationJacobian(df_dpose);
578 
579  // Numerical approximation:
581  {
582  CVectorFixedDouble<4> x_mean;
583  for (int i = 0; i < 4; i++) x_mean[i] = q1[i];
584 
585  double DUMMY = 0;
586  CVectorFixedDouble<4> x_incrs;
587  x_incrs.fill(1e-5);
588  CMatrixDouble numJacobs;
590  x_mean,
591  std::function<void(
592  const CVectorFixedDouble<4>& x, const double& dummy,
593  CVectorFixedDouble<4>& Y)>(&func_normalizeJacob),
594  x_incrs, DUMMY, numJacobs);
595 
596  num_df_dpose = numJacobs.block<4, 4>(0, 0);
597  }
598 
599  // Compare:
600  EXPECT_NEAR(
601  0,
602  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
603  3e-3)
604  << "q1: " << q1 << endl
605  << "Numeric approximation of df_dpose: " << endl
606  << num_df_dpose.asEigen() << endl
607  << "Implemented method: " << endl
608  << df_dpose.asEigen() << endl
609  << "Error: " << endl
610  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
611  }
612 };
613 
614 TEST_F(Pose3DQuatTests, FromYPRAndBack)
615 {
616  test_fromYPRAndBack(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
617  test_fromYPRAndBack(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
618  test_fromYPRAndBack(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
619  test_fromYPRAndBack(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
620  test_fromYPRAndBack(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
621  test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
622  test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
623 }
624 
625 TEST_F(Pose3DQuatTests, UnaryInverse)
626 {
627  test_unaryInverse(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
628  test_unaryInverse(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
629  test_unaryInverse(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
630  test_unaryInverse(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
631  test_unaryInverse(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
632  test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
633  test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
634 }
635 
636 TEST_F(Pose3DQuatTests, CopyOperator)
637 {
638  test_copy(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
639  test_copy(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
640  test_copy(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
641  test_copy(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
642 }
643 
645 {
646  test_compose(
647  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 2.0, -5.0, 8.0, 40.0_deg,
648  -5.0_deg, 25.0_deg);
649 
650  test_compose(
651  25.0, 2.0, 3.0, -30.0_deg, 90.0_deg, 0.0_deg, -10.0, 4.0, -8.0,
652  20.0_deg, 9.0_deg, 0.0_deg);
653 }
654 
655 TEST_F(Pose3DQuatTests, ComposeWithPoint)
656 {
657  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
658  test_composePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
659  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
660  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
661  test_composePoint(
662  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
663  test_composePoint(
664  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
665 }
666 
667 TEST_F(Pose3DQuatTests, ComposeWithPointJacob)
668 {
669  test_composePointJacob(
670  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
671  test_composePointJacob(
672  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
673  test_composePointJacob(
674  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
675  test_composePointJacob(
676  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
677  test_composePointJacob(
678  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
679  test_composePointJacob(
680  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
681 }
682 
683 TEST_F(Pose3DQuatTests, InvComposeWithPoint)
684 {
685  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
686  test_invComposePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
687  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
688  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
689  test_invComposePoint(
690  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
691  test_invComposePoint(
692  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
693 }
694 
695 TEST_F(Pose3DQuatTests, InvComposeWithPointJacob)
696 {
697  test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
698  test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 1, 2, 3);
699  test_invComposePointJacob(
700  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
701  test_invComposePointJacob(
702  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
703  test_invComposePointJacob(
704  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
705  test_invComposePointJacob(
706  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
707  test_invComposePointJacob(
708  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
709  test_invComposePointJacob(
710  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
711  test_invComposePointJacob(
712  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
713 }
714 
715 TEST_F(Pose3DQuatTests, ComposeInvComposePoint)
716 {
717  test_composeAndInvComposePoint(
718  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
719  test_composeAndInvComposePoint(
720  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
721  test_composeAndInvComposePoint(
722  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
723  test_composeAndInvComposePoint(
724  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
725  test_composeAndInvComposePoint(
726  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
727  test_composeAndInvComposePoint(
728  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
729 }
730 
731 TEST_F(Pose3DQuatTests, ComposePoint_vs_CPose3D)
732 {
733  test_composePoint_vs_CPose3D(
734  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
735  test_composePoint_vs_CPose3D(
736  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
737  test_composePoint_vs_CPose3D(
738  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
739  test_composePoint_vs_CPose3D(
740  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
741  test_composePoint_vs_CPose3D(
742  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
743  test_composePoint_vs_CPose3D(
744  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
745 }
746 
747 TEST_F(Pose3DQuatTests, InvComposePoint_vs_CPose3D)
748 {
749  test_invComposePoint_vs_CPose3D(
750  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
751  test_invComposePoint_vs_CPose3D(
752  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
753  test_invComposePoint_vs_CPose3D(
754  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
755  test_invComposePoint_vs_CPose3D(
756  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
757 
758  for (size_t i = 0; i < 10; i++)
759  {
760  std::vector<double> v(9);
762  test_invComposePoint_vs_CPose3D(
763  v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]);
764  }
765 }
766 
767 TEST_F(Pose3DQuatTests, SphericalCoordsJacobian)
768 {
769  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
770  test_sphericalCoords(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
771  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
772  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
773  test_sphericalCoords(
774  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
775  test_sphericalCoords(
776  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
777 }
778 
779 TEST_F(Pose3DQuatTests, NormalizationJacobian)
780 {
781  test_normalizeJacob(0.0_deg, 0.0_deg, 0.0_deg);
782  test_normalizeJacob(10.0_deg, 0.0_deg, 0.0_deg);
783  test_normalizeJacob(0.0_deg, 10.0_deg, 0.0_deg);
784  test_normalizeJacob(0.0_deg, 0.0_deg, 10.0_deg);
785  test_normalizeJacob(-30.0_deg, 10.0_deg, 60.0_deg);
786  test_normalizeJacob(10.0_deg, -50.0_deg, -40.0_deg);
787 }
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:58
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void test_composePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
void test_fromYPRAndBack(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
void drawGaussian1DVector(VEC &v, const double mean=0, const double std=1)
Fills the given vector with independent, 1D-normally distributed samples.
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:89
static void func_spherical_coords(const CVectorFixedDouble< 7+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:505
void test_copy(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
STL namespace.
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacob_dryp_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacob_dryp_dpose=nullptr) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
static void func_normalizeJacob(const CVectorFixedDouble< 4 > &x, const double &dummy, CVectorFixedDouble< 4 > &Y)
void TearDown() override
static void func_compose_point(const CVectorFixedDouble< 7+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::optional_ref< mrpt::math::CMatrixDouble43 > out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:517
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point G such as .
This base provides a set of functions for maths stuff.
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:85
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
T square(const T x)
Inline function for the square of a number.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
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:312
void test_invComposePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
void test_composeAndInvComposePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
static void func_inv_compose_point(const CVectorFixedDouble< 7+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:31
void test_invComposePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
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)
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:87
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:286
void SetUp() override
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
void test_composePoint_vs_CPose3D(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
GLenum GLint GLint y
Definition: glext.h:3542
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, 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...
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:278
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
GLenum GLint x
Definition: glext.h:3542
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
Lightweight 3D point.
Definition: TPoint3D.h:90
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:91
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
void test_unaryInverse(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
GLfloat GLfloat p
Definition: glext.h:6398
void test_composePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
TEST_F(Pose3DQuatTests, FromYPRAndBack)
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:266
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019