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 // Check yaw-pitch-roll [rad] vs its [qx,qy,qz,qw] representation:
615 static void quat_vs_YPR(
616  double yaw, double pitch, double roll, double qx, double qy, double qz,
617  double qw, const std::string& sRotMat)
618 {
619  const double eps = 1e-4;
620 
621  // First, test Yaw-pitch-roll to Rot matrix:
622  const mrpt::poses::CPose3D p(0, 0, 0, yaw, pitch, roll);
624  std::stringstream sErr;
625  if (!R_gt.fromMatlabStringFormat(sRotMat, sErr))
626  GTEST_FAIL() << "Incorrect R_gt matrix: '" << sRotMat
627  << "'. Error: " << sErr.str() << "\n";
628  EXPECT_EQ(R_gt.cols(), 3) << " for: sRotMat='" << sRotMat << "'\n";
629  EXPECT_EQ(R_gt.rows(), 3) << " for: sRotMat='" << sRotMat << "'\n";
630 
631  EXPECT_NEAR(
632  0,
633  (R_gt.asEigen() - p.getRotationMatrix().asEigen())
634  .array()
635  .abs()
636  .maxCoeff(),
637  eps)
638  << "R_gt=\n"
639  << R_gt << "\np.R=\n"
640  << p.getRotationMatrix() << std::endl;
641 
642  // Convert to quat:
643  const auto q_gt = mrpt::math::CQuaternionDouble(qw, qx, qy, qz);
645  p.getAsQuaternion(q);
646 
647  if (std::abs(q.w() - q_gt.w()) > eps || std::abs(q.x() - q_gt.x()) > eps ||
648  std::abs(q.y() - q_gt.y()) > eps || std::abs(q.z() - q_gt.z()) > eps)
649  {
650  GTEST_FAIL() << "q = " << q.asString()
651  << "\nExpected = " << q_gt.asString() << "\n";
652  }
653 
655  EXPECT_NEAR(
656  0,
657  (R.asEigen() - p.getRotationMatrix().asEigen())
658  .array()
659  .abs()
660  .maxCoeff(),
661  eps)
662  << "q.R=\n"
663  << R << "\np.R=" << p.getRotationMatrix() << std::endl;
664 }
665 
666 // Check yaw-pitch-roll vs its [qx,qy,qz,qw] representation:
667 TEST(QuatTests, Quat_vs_YPR)
668 {
669  // Ground truth values obtained from:
670  // https://www.andre-gaschler.com/rotationconverter/
671  // Using: ZYX Euler angles convention
672 
673  quat_vs_YPR(
674  0.0_deg, 0.0_deg, 0.0_deg, // Input Yaw-pitch-roll
675  0, 0, 0, 1, // Expected quaternion
676  "[ 1.0000000 0.0000000 0.0000000; "
677  " 0.0000000 1.0000000 0.0000000; "
678  " 0.0000000 0.0000000 1.0000000 ]");
679  quat_vs_YPR(
680  90.0_deg, 0.0_deg, 0.0_deg, // Input Yaw-pitch-roll
681  0, 0, 0.7071068, 0.7071068, // Expected quaternion
682  "[ 0.0000000 -1.0000000 0.0000000;"
683  " 1.0000000 0.0000000 0.0000000;"
684  " 0.0000000 0.0000000 1.0000000 ]");
685  quat_vs_YPR(
686  30.0_deg, 10.0_deg, 60.0_deg, // Input Yaw-pitch-roll
687  0.4615897, 0.2018243, 0.1811979, 0.8446119, // Expected quaternion
688  "[ 0.8528686 -0.1197639 0.5082046;"
689  " 0.4924039 0.5082046 -0.7065880;"
690  " -0.1736482 0.8528686 0.4924039 ]");
691  quat_vs_YPR(
692  -10.0_deg, -20.0_deg, -30.0_deg, // Input Yaw-pitch-roll
693  -0.2685358, -0.1448781, -0.1276794, 0.9437144, // Expected quaternion
694  "[ 0.9254166 0.3187958 -0.2048741;"
695  " -0.1631759 0.8231729 0.5438381;"
696  " 0.3420202 -0.4698463 0.8137977 ]");
697  quat_vs_YPR(
698  -179.9995949_deg, -90.0_deg, 0.0_deg, // Input Yaw-pitch-roll
699  0.7071068, 0, 0.7071068, -0.000005, // Expected quaternion
700  "[ 0.0000000 0.0000071 1.0000000;"
701  " -0.0000071 -1.0000000 0.0000071;"
702  " 1.0000000 -0.0000071 0.0000000 ]");
703 }
704 
705 TEST_F(Pose3DQuatTests, FromYPRAndBack)
706 {
707  test_fromYPRAndBack(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
708  test_fromYPRAndBack(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
709  test_fromYPRAndBack(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
710  test_fromYPRAndBack(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
711  test_fromYPRAndBack(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
712  test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
713  test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
714 }
715 
716 TEST_F(Pose3DQuatTests, UnaryInverse)
717 {
718  test_unaryInverse(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
719  test_unaryInverse(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
720  test_unaryInverse(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
721  test_unaryInverse(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
722  test_unaryInverse(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
723  test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
724  test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
725 }
726 
727 TEST_F(Pose3DQuatTests, CopyOperator)
728 {
729  test_copy(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
730  test_copy(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
731  test_copy(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
732  test_copy(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
733 }
734 
736 {
737  test_compose(
738  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 2.0, -5.0, 8.0, 40.0_deg,
739  -5.0_deg, 25.0_deg);
740 
741  test_compose(
742  25.0, 2.0, 3.0, -30.0_deg, 90.0_deg, 0.0_deg, -10.0, 4.0, -8.0,
743  20.0_deg, 9.0_deg, 0.0_deg);
744 }
745 
746 TEST_F(Pose3DQuatTests, ComposeWithPoint)
747 {
748  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
749  test_composePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
750  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
751  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
752  test_composePoint(
753  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
754  test_composePoint(
755  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
756 }
757 
758 TEST_F(Pose3DQuatTests, ComposeWithPointJacob)
759 {
760  test_composePointJacob(
761  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
762  test_composePointJacob(
763  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
764  test_composePointJacob(
765  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
766  test_composePointJacob(
767  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
768  test_composePointJacob(
769  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
770  test_composePointJacob(
771  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
772 }
773 
774 TEST_F(Pose3DQuatTests, InvComposeWithPoint)
775 {
776  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
777  test_invComposePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
778  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
779  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
780  test_invComposePoint(
781  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
782  test_invComposePoint(
783  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
784 }
785 
786 TEST_F(Pose3DQuatTests, InvComposeWithPointJacob)
787 {
788  test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
789  test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 1, 2, 3);
790  test_invComposePointJacob(
791  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
792  test_invComposePointJacob(
793  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
794  test_invComposePointJacob(
795  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
796  test_invComposePointJacob(
797  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
798  test_invComposePointJacob(
799  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
800  test_invComposePointJacob(
801  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
802  test_invComposePointJacob(
803  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
804 }
805 
806 TEST_F(Pose3DQuatTests, ComposeInvComposePoint)
807 {
808  test_composeAndInvComposePoint(
809  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
810  test_composeAndInvComposePoint(
811  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
812  test_composeAndInvComposePoint(
813  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
814  test_composeAndInvComposePoint(
815  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
816  test_composeAndInvComposePoint(
817  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
818  test_composeAndInvComposePoint(
819  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
820 }
821 
822 TEST_F(Pose3DQuatTests, ComposePoint_vs_CPose3D)
823 {
824  test_composePoint_vs_CPose3D(
825  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
826  test_composePoint_vs_CPose3D(
827  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
828  test_composePoint_vs_CPose3D(
829  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
830  test_composePoint_vs_CPose3D(
831  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
832  test_composePoint_vs_CPose3D(
833  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
834  test_composePoint_vs_CPose3D(
835  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
836 }
837 
838 TEST_F(Pose3DQuatTests, InvComposePoint_vs_CPose3D)
839 {
840  test_invComposePoint_vs_CPose3D(
841  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
842  test_invComposePoint_vs_CPose3D(
843  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
844  test_invComposePoint_vs_CPose3D(
845  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
846  test_invComposePoint_vs_CPose3D(
847  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
848 
849  for (size_t i = 0; i < 10; i++)
850  {
851  std::vector<double> v(9);
853  test_invComposePoint_vs_CPose3D(
854  v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]);
855  }
856 }
857 
858 TEST_F(Pose3DQuatTests, SphericalCoordsJacobian)
859 {
860  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
861  test_sphericalCoords(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
862  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
863  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
864  test_sphericalCoords(
865  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
866  test_sphericalCoords(
867  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
868 }
869 
870 TEST_F(Pose3DQuatTests, NormalizationJacobian)
871 {
872  test_normalizeJacob(0.0_deg, 0.0_deg, 0.0_deg);
873  test_normalizeJacob(10.0_deg, 0.0_deg, 0.0_deg);
874  test_normalizeJacob(0.0_deg, 10.0_deg, 0.0_deg);
875  test_normalizeJacob(0.0_deg, 0.0_deg, 10.0_deg);
876  test_normalizeJacob(-30.0_deg, 10.0_deg, 60.0_deg);
877  test_normalizeJacob(10.0_deg, -50.0_deg, -40.0_deg);
878 }
static void quat_vs_YPR(double yaw, double pitch, double roll, double qx, double qy, double qz, double qw, const std::string &sRotMat)
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
void rotationMatrix(MATRIXLIKE &M) const
Calculate the 3x3 rotation matrix associated to this quaternion: .
Definition: CQuaternion.h:380
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)
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:301
std::string asString() const
Returns a string representation of the vector/matrix, using Eigen&#39;s default settings.
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:107
static void func_spherical_coords(const CVectorFixedDouble< 7+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
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:540
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 (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
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 .
TEST(QuatTests, Quat_vs_YPR)
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)
const double eps
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:242
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
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
constexpr size_type rows() const
Number of rows in the matrix.
Definition: CMatrixFixed.h:227
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)
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:23
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:105
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:313
void SetUp() override
const float R
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)
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
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)
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...
constexpr size_type cols() const
Number of columns in the matrix.
Definition: CMatrixFixed.h:230
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)
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109
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.
T w() const
Return w (real part) coordinate of the quaternion.
Definition: CQuaternion.h:103
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)
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: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020