MRPT  2.0.4
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-2020, 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 using namespace mrpt::math;
21 using namespace std;
22 
23 template class mrpt::CTraitsTest<CPose3DQuat>;
24 
25 class Pose3DQuatTests : public ::testing::Test
26 {
27  protected:
28  void SetUp() override {}
29  void TearDown() override {}
30  void test_compose(
31  double x1, double y1, double z1, double yaw1, double pitch1,
32  double roll1, double x2, double y2, double z2, double yaw2,
33  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  CPose3DQuat q1 = CPose3DQuat(p1);
42  CPose3DQuat q2 = CPose3DQuat(p2);
43 
44  CPose3DQuat q1_c_q2 = q1 + q2;
45  CPose3DQuat q1_i_q2 = q1 - q2;
46 
47  CPose3D p_q1_c_q2 = CPose3D(q1_c_q2);
48  CPose3D p_q1_i_q2 = CPose3D(q1_i_q2);
49 
50  EXPECT_NEAR(
51  0,
52  (p1_c_p2.asVectorVal() - p_q1_c_q2.asVectorVal())
53  .array()
54  .abs()
55  .sum(),
56  1e-5)
57  << "p1_c_p2: " << p1_c_p2 << endl
58  << "q1_c_p2: " << p_q1_c_q2 << endl;
59 
60  EXPECT_NEAR(
61  0,
62  (p1_i_p2.asVectorVal() - p_q1_i_q2.asVectorVal())
63  .array()
64  .abs()
65  .sum(),
66  1e-5)
67  << "p1_i_p2: " << p1_i_p2 << endl
68  << "q1_i_p2: " << p_q1_i_q2 << endl;
69 
70  // Test + operator: trg new var
71  {
72  CPose3DQuat C = q1;
73  CPose3DQuat A = C + q2;
74  EXPECT_NEAR(
75  0,
76  (A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
77  1e-6);
78  }
79  // Test + operator: trg same var
80  {
81  CPose3DQuat A = q1;
82  A = A + q2;
83  EXPECT_NEAR(
84  0,
85  (A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
86  1e-6);
87  }
88  // Test =+ operator
89  {
90  CPose3DQuat A = q1;
91  A += q2;
92  EXPECT_NEAR(
93  0,
94  (A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
95  1e-6);
96  }
97  }
98 
99  void test_composePoint(
100  double x1, double y1, double z1, double yaw1, double pitch1,
101  double roll1, double x, double y, double z)
102  {
103  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
104  const CPose3DQuat q1(p1);
105  const CPoint3D p(x, y, z);
106 
107  CPoint3D p1_plus_p = p1 + p;
108  CPoint3D q1_plus_p = q1 + p;
109 
110  EXPECT_NEAR(
111  0,
112  (p1_plus_p.asVectorVal() - q1_plus_p.asVectorVal())
113  .array()
114  .abs()
115  .sum(),
116  1e-5)
117  << "p1: " << p1 << endl
118  << "q1: " << q1 << endl
119  << "p: " << p << endl
120  << "p1_plus_p: " << p1_plus_p << endl
121  << "q1_plus_p: " << q1_plus_p << endl;
122  }
123 
124  static void func_compose_point(
125  const CVectorFixedDouble<7 + 3>& x,
126  [[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
127  {
128  CPose3DQuat q(
129  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
130  q.quat().normalize();
131  const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
132  const CPoint3D pp = q + p;
133  for (int i = 0; i < 3; i++) Y[i] = pp[i];
134  }
135 
136  void test_composePointJacob(
137  double x1, double y1, double z1, double yaw1, double pitch1,
138  double roll1, double x, double y, double z)
139  {
140  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
141  const CPoint3D p(x, y, z);
142 
143  CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
144  CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
145 
146  TPoint3D l;
147  q1.composePoint(x, y, z, l.x, l.y, l.z, &df_dpoint, &df_dpose);
148 
149  // Numerical approximation:
150  CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
151  CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
152  {
153  CVectorFixedDouble<7 + 3> x_mean;
154  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
155  x_mean[7 + 0] = x;
156  x_mean[7 + 1] = y;
157  x_mean[7 + 2] = z;
158 
159  double DUMMY = 0;
160  CVectorFixedDouble<7 + 3> x_incrs;
161  x_incrs.fill(1e-7);
162  CMatrixDouble numJacobs;
163  mrpt::math::estimateJacobian(
164  x_mean,
165  std::function<void(
166  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
167  CVectorFixedDouble<3>& Y)>(&func_compose_point),
168  x_incrs, DUMMY, numJacobs);
169 
170  num_df_dpose = numJacobs.asEigen().block<3, 7>(0, 0);
171  num_df_dpoint = numJacobs.asEigen().block<3, 3>(0, 7);
172  }
173 
174  // Compare:
175  EXPECT_NEAR(
176  0,
177  (df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
178  3e-3)
179  << "q1: " << q1 << endl
180  << "p: " << p << endl
181  << "Numeric approximation of df_dpoint: " << endl
182  << num_df_dpoint.asEigen() << endl
183  << "Implemented method: " << endl
184  << df_dpoint << endl
185  << "Error: " << endl
186  << df_dpoint.asEigen() - num_df_dpoint.asEigen() << endl;
187 
188  EXPECT_NEAR(
189  0,
190  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
191  3e-3)
192  << "q1: " << q1 << endl
193  << "p: " << p << endl
194  << "Numeric approximation of df_dpose: " << endl
195  << num_df_dpose.asEigen() << endl
196  << "Implemented method: " << endl
197  << df_dpose.asEigen() << endl
198  << "Error: " << endl
199  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
200  }
201 
202  void test_invComposePoint(
203  double x1, double y1, double z1, double yaw1, double pitch1,
204  double roll1, double x, double y, double z)
205  {
206  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
207  const CPose3DQuat q1(p1);
208  const CPoint3D p(x, y, z);
209 
210  CPoint3D p_minus_p1 = p - p1;
211  CPoint3D p_minus_q1 = p - q1;
212 
213  CPoint3D p_rec = q1 + p_minus_q1;
214 
215  EXPECT_NEAR(
216  0,
217  (p_minus_p1.asVectorVal() - p_minus_q1.asVectorVal())
218  .array()
219  .abs()
220  .sum(),
221  1e-5)
222  << "p_minus_p1: " << p_minus_p1 << endl
223  << "p_minus_q1: " << p_minus_q1 << endl;
224 
225  EXPECT_NEAR(
226  0, (p_rec.asVectorVal() - p.asVectorVal()).array().abs().sum(),
227  1e-5)
228  << "p_rec: " << p_rec << endl
229  << "p: " << p << endl;
230  }
231 
232  static void func_inv_compose_point(
233  const CVectorFixedDouble<7 + 3>& x,
234  [[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
235  {
236  CPose3DQuat q(
237  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
238  q.quat().normalize();
239  const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
240  const CPoint3D pp = p - q;
241  Y[0] = pp.x();
242  Y[1] = pp.y();
243  Y[2] = pp.z();
244  }
245 
246  void test_invComposePointJacob(
247  double x1, double y1, double z1, double yaw1, double pitch1,
248  double roll1, double x, double y, double z)
249  {
250  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
251  const CPoint3D p(x, y, z);
252 
253  CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
254  CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
255 
256  TPoint3D l;
257  q1.inverseComposePoint(x, y, z, l.x, l.y, l.z, &df_dpoint, &df_dpose);
258 
259  // Also check the returned point, not just the jacobian:
260  TPoint3D theorical;
261  {
262  const double qr = q1.quat().r();
263  const double qx = q1.quat().x();
264  const double qy = q1.quat().y();
265  const double qz = q1.quat().z();
266  const double Ax = x - x1; // ax - x;
267  const double Ay = y - y1; // ay - y;
268  const double Az = z - z1; // az - z;
269  theorical.x = Ax + 2 * (Ay) * (qr * qz + qx * qy) -
270  2 * (Az) * (qr * qy - qx * qz) -
271  2 * (square(qy) + square(qz)) * (Ax);
272  theorical.y = Ay - 2 * (Ax) * (qr * qz - qx * qy) +
273  2 * (Az) * (qr * qx + qy * qz) -
274  2 * (square(qx) + square(qz)) * (Ay);
275  theorical.z = Az + 2 * (Ax) * (qr * qy + qx * qz) -
276  2 * (Ay) * (qr * qx - qy * qz) -
277  2 * (square(qx) + square(qy)) * (Az);
278  }
279  EXPECT_NEAR(theorical.x, l.x, 1e-5);
280  EXPECT_NEAR(theorical.y, l.y, 1e-5);
281  EXPECT_NEAR(theorical.z, l.z, 1e-5);
282 
283  // Numerical approximation:
284  CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
285  CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
286  {
287  CVectorFixedDouble<7 + 3> x_mean;
288  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
289  x_mean[7 + 0] = x;
290  x_mean[7 + 1] = y;
291  x_mean[7 + 2] = z;
292 
293  double DUMMY = 0;
294  CVectorFixedDouble<7 + 3> x_incrs;
295  x_incrs.fill(1e-7);
296  CMatrixDouble numJacobs;
297  mrpt::math::estimateJacobian(
298  x_mean,
299  std::function<void(
300  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
301  CVectorFixedDouble<3>& Y)>(&func_inv_compose_point),
302  x_incrs, DUMMY, numJacobs);
303 
304  num_df_dpose = numJacobs.block<3, 7>(0, 0);
305  num_df_dpoint = numJacobs.block<3, 3>(0, 7);
306  }
307 
308  // Compare:
309  EXPECT_NEAR(
310  0,
311  (df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
312  3e-3)
313  << "q1: " << q1 << endl
314  << "from pose: " << CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
315  << "p: " << p << endl
316  << "local: " << l << endl
317  << "Numeric approximation of df_dpoint: " << endl
318  << num_df_dpoint.asEigen() << endl
319  << "Implemented method: " << endl
320  << df_dpoint.asEigen() << endl
321  << "Error: " << endl
322  << df_dpoint - num_df_dpoint << endl;
323 
324  EXPECT_NEAR(
325  0,
326  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
327  3e-3)
328  << "q1: " << q1 << endl
329  << "from pose: " << CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
330  << "p: " << p << endl
331  << "local: " << l << endl
332  << "Numeric approximation of df_dpose: " << endl
333  << num_df_dpose.asEigen() << endl
334  << "Implemented method: " << endl
335  << df_dpose.asEigen() << endl
336  << "Error: " << endl
337  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
338  }
339 
340  void test_fromYPRAndBack(
341  double x1, double y1, double z1, double yaw1, double pitch1,
342  double roll1)
343  {
344  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
345  const CPose3DQuat q1(p1);
346  const CPose3D p1r = CPose3D(q1);
347 
348  EXPECT_NEAR(
349  0,
350  (p1.getHomogeneousMatrixVal<CMatrixDouble44>() -
351  q1.getHomogeneousMatrixVal<CMatrixDouble44>())
352  .array()
353  .abs()
354  .sum(),
355  1e-5)
356  << "p1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
357  << p1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
358  << "q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
359  << q1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
360 
361  EXPECT_NEAR(
362  0, (p1.asVectorVal() - p1r.asVectorVal()).array().abs().sum(), 1e-5)
363  << "p1: " << p1 << endl
364  << "q1: " << q1 << endl
365  << "p1r: " << p1r << endl;
366  }
367 
368  void test_unaryInverse(
369  double x1, double y1, double z1, double yaw1, double pitch1,
370  double roll1)
371  {
372  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
373  const CPose3D p1_inv = -p1;
374 
375  const CPose3DQuat q1(p1);
376  const CPose3DQuat q1_inv = -q1;
377 
378  EXPECT_NEAR(
379  0,
380  (p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() -
381  q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>())
382  .array()
383  .abs()
384  .sum(),
385  1e-5)
386  << "p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
387  << p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
388  << "q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
389  << q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
390  }
391 
392  void test_copy(
393  double x1, double y1, double z1, double yaw1, double pitch1,
394  double roll1)
395  {
396  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
397 
398  const CPose3DQuat q1(p1);
399  const CPose3DQuat q2 = q1;
400 
401  EXPECT_NEAR(
402  0,
403  (q1.getHomogeneousMatrixVal<CMatrixDouble44>() -
404  q2.getHomogeneousMatrixVal<CMatrixDouble44>())
405  .array()
406  .abs()
407  .sum(),
408  1e-5)
409  << "q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
410  << q1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
411  << "q2.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
412  << q2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
413  }
414 
415  void test_composeAndInvComposePoint(
416  double x1, double y1, double z1, double yaw1, double pitch1,
417  double roll1, double x, double y, double z)
418  {
419  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
420  TPoint3D pp(x, y, z), aux;
421  q1.composePoint(x, y, z, pp.x, pp.y, pp.z);
422  q1.inverseComposePoint(pp.x, pp.y, pp.z, aux.x, aux.y, aux.z);
423 
424  EXPECT_NEAR(x, aux.x, 1e-7);
425  EXPECT_NEAR(y, aux.y, 1e-7);
426  EXPECT_NEAR(z, aux.z, 1e-7);
427  }
428 
429  void test_composePoint_vs_CPose3D(
430  double x1, double y1, double z1, double yaw1, double pitch1,
431  double roll1, double x, double y, double z)
432  {
433  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
434  const CPose3DQuat q1(p1);
435  TPoint3D pt1, pt2;
436  p1.composePoint(x, y, z, pt1.x, pt1.y, pt1.z);
437  q1.composePoint(x, y, z, pt2.x, pt2.y, pt2.z);
438 
439  EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
440  EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
441  EXPECT_NEAR(pt1.z, pt2.z, 1e-7);
442  }
443 
444  void test_invComposePoint_vs_CPose3D(
445  double x1, double y1, double z1, double yaw1, double pitch1,
446  double roll1, double x, double y, double z)
447  {
448  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
449  const CPose3DQuat q1(p1);
450  TPoint3D pt1, pt2;
451  p1.inverseComposePoint(x, y, z, pt1.x, pt1.y, pt1.z);
452  q1.inverseComposePoint(x, y, z, pt2.x, pt2.y, pt2.z);
453 
454  EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
455  EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
456  EXPECT_NEAR(pt1.z, pt2.z, 1e-7);
457 
458  {
459  CPose3DQuat q = mrpt::poses::CPose3DQuat(p1);
460 
461  double gx = x, gy = y, gz = z;
462 
463  double dist, yaw, pitch;
464  p1.sphericalCoordinates(TPoint3D(gx, gy, gz), dist, yaw, pitch);
465 
466  double lx, ly, lz;
467  p1.inverseComposePoint(gx, gy, gz, lx, ly, lz);
468 
469  double lx2, ly2, lz2;
470  q.inverseComposePoint(gx, gy, gz, lx2, ly2, lz2);
471 
472  EXPECT_NEAR(lx, lx2, 1e-7);
473  EXPECT_NEAR(ly, ly2, 1e-7);
474  EXPECT_NEAR(lz, lz2, 1e-7);
475  }
476  }
477 
478  static void func_spherical_coords(
479  const CVectorFixedDouble<7 + 3>& x,
480  [[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
481  {
482  CPose3DQuat q(
483  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
484  q.quat().normalize();
485  const TPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
486  q.sphericalCoordinates(p, Y[0], Y[1], Y[2]);
487  }
488 
489  void test_sphericalCoords(
490  double x1, double y1, double z1, double yaw1, double pitch1,
491  double roll1, double x, double y, double z)
492  {
493  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
494  const TPoint3D p(x, y, z);
495 
496  CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
497  CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
498 
499  double hr, hy, hp;
500  q1.sphericalCoordinates(p, hr, hy, hp, &df_dpoint, &df_dpose);
501 
502  // Numerical approximation:
503  CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
504  CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
505  {
506  CVectorFixedDouble<7 + 3> x_mean;
507  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
508  x_mean[7 + 0] = x;
509  x_mean[7 + 1] = y;
510  x_mean[7 + 2] = z;
511 
512  double DUMMY = 0;
513  CVectorFixedDouble<7 + 3> x_incrs;
514  x_incrs.fill(1e-7);
515  CMatrixDouble numJacobs;
516  mrpt::math::estimateJacobian(
517  x_mean,
518  std::function<void(
519  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
520  CVectorFixedDouble<3>& Y)>(&func_spherical_coords),
521  x_incrs, DUMMY, numJacobs);
522 
523  num_df_dpose = numJacobs.block<3, 7>(0, 0);
524  num_df_dpoint = numJacobs.block<3, 3>(0, 7);
525  }
526 
527  // Compare:
528  EXPECT_NEAR(
529  0,
530  (df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
531  3e-3)
532  << "q1: " << q1 << endl
533  << "p: " << p << endl
534  << "Numeric approximation of df_dpoint: " << endl
535  << num_df_dpoint.asEigen() << endl
536  << "Implemented method: " << endl
537  << df_dpoint.asEigen() << endl
538  << "Error: " << endl
539  << df_dpoint.asEigen() - num_df_dpoint.asEigen() << endl;
540 
541  EXPECT_NEAR(
542  0,
543  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
544  3e-3)
545  << "q1: " << q1 << endl
546  << "p: " << p << endl
547  << "Numeric approximation of df_dpose: " << endl
548  << num_df_dpose.asEigen() << endl
549  << "Implemented method: " << endl
550  << df_dpose.asEigen() << endl
551  << "Error: " << endl
552  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
553  }
554 
555  static void func_normalizeJacob(
556  const CVectorFixedDouble<4>& x, [[maybe_unused]] const double& dummy,
557  CVectorFixedDouble<4>& Y)
558  {
559  CQuaternionDouble q;
560  for (int i = 0; i < 4; i++) q[i] = x[i];
561  q.normalize();
562  for (int i = 0; i < 4; i++) Y[i] = q[i];
563  }
564 
565  void test_normalizeJacob(double yaw1, double pitch1, double roll1)
566  {
567  const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
568  CQuaternionDouble q1;
569  pp.getAsQuaternion(q1);
570 
571  CMatrixFixed<double, 4, 4> df_dpose(UNINITIALIZED_MATRIX);
572  q1.normalizationJacobian(df_dpose);
573 
574  // Numerical approximation:
575  CMatrixFixed<double, 4, 4> num_df_dpose(UNINITIALIZED_MATRIX);
576  {
577  CVectorFixedDouble<4> x_mean;
578  for (int i = 0; i < 4; i++) x_mean[i] = q1[i];
579 
580  double DUMMY = 0;
581  CVectorFixedDouble<4> x_incrs;
582  x_incrs.fill(1e-5);
583  CMatrixDouble numJacobs;
584  mrpt::math::estimateJacobian(
585  x_mean,
586  std::function<void(
587  const CVectorFixedDouble<4>& x, const double& dummy,
588  CVectorFixedDouble<4>& Y)>(&func_normalizeJacob),
589  x_incrs, DUMMY, numJacobs);
590 
591  num_df_dpose = numJacobs.block<4, 4>(0, 0);
592  }
593 
594  // Compare:
595  EXPECT_NEAR(
596  0,
597  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
598  3e-3)
599  << "q1: " << q1 << endl
600  << "Numeric approximation of df_dpose: " << endl
601  << num_df_dpose.asEigen() << endl
602  << "Implemented method: " << endl
603  << df_dpose.asEigen() << endl
604  << "Error: " << endl
605  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
606  }
607 };
608 
609 // Check yaw-pitch-roll [rad] vs its [qx,qy,qz,qw] representation:
610 static void quat_vs_YPR(
611  double yaw, double pitch, double roll, double qx, double qy, double qz,
612  double qw, const std::string& sRotMat)
613 {
614  const double eps = 1e-4;
615 
616  // First, test Yaw-pitch-roll to Rot matrix:
617  const mrpt::poses::CPose3D p(0, 0, 0, yaw, pitch, roll);
618  mrpt::math::CMatrixDouble33 R_gt;
619  std::stringstream sErr;
620  if (!R_gt.fromMatlabStringFormat(sRotMat, sErr))
621  GTEST_FAIL() << "Incorrect R_gt matrix: '" << sRotMat
622  << "'. Error: " << sErr.str() << "\n";
623  EXPECT_EQ(R_gt.cols(), 3) << " for: sRotMat='" << sRotMat << "'\n";
624  EXPECT_EQ(R_gt.rows(), 3) << " for: sRotMat='" << sRotMat << "'\n";
625 
626  EXPECT_NEAR(
627  0,
628  (R_gt.asEigen() - p.getRotationMatrix().asEigen())
629  .array()
630  .abs()
631  .maxCoeff(),
632  eps)
633  << "R_gt=\n"
634  << R_gt << "\np.R=\n"
635  << p.getRotationMatrix() << std::endl;
636 
637  // Convert to quat:
638  const auto q_gt = mrpt::math::CQuaternionDouble(qw, qx, qy, qz);
639  mrpt::math::CQuaternionDouble q;
640  p.getAsQuaternion(q);
641 
642  if (std::abs(q.w() - q_gt.w()) > eps || std::abs(q.x() - q_gt.x()) > eps ||
643  std::abs(q.y() - q_gt.y()) > eps || std::abs(q.z() - q_gt.z()) > eps)
644  {
645  GTEST_FAIL() << "q = " << q.asString()
646  << "\nExpected = " << q_gt.asString() << "\n";
647  }
648 
649  auto R = q.rotationMatrix<mrpt::math::CMatrixDouble33>();
650  EXPECT_NEAR(
651  0,
652  (R.asEigen() - p.getRotationMatrix().asEigen())
653  .array()
654  .abs()
655  .maxCoeff(),
656  eps)
657  << "q.R=\n"
658  << R << "\np.R=" << p.getRotationMatrix() << std::endl;
659 }
660 
661 // Check yaw-pitch-roll vs its [qx,qy,qz,qw] representation:
662 TEST(QuatTests, Quat_vs_YPR)
663 {
664  // Ground truth values obtained from:
665  // https://www.andre-gaschler.com/rotationconverter/
666  // Using: ZYX Euler angles convention
667 
668  quat_vs_YPR(
669  0.0_deg, 0.0_deg, 0.0_deg, // Input Yaw-pitch-roll
670  0, 0, 0, 1, // Expected quaternion
671  "[ 1.0000000 0.0000000 0.0000000; "
672  " 0.0000000 1.0000000 0.0000000; "
673  " 0.0000000 0.0000000 1.0000000 ]");
674  quat_vs_YPR(
675  90.0_deg, 0.0_deg, 0.0_deg, // Input Yaw-pitch-roll
676  0, 0, 0.7071068, 0.7071068, // Expected quaternion
677  "[ 0.0000000 -1.0000000 0.0000000;"
678  " 1.0000000 0.0000000 0.0000000;"
679  " 0.0000000 0.0000000 1.0000000 ]");
680  quat_vs_YPR(
681  30.0_deg, 10.0_deg, 60.0_deg, // Input Yaw-pitch-roll
682  0.4615897, 0.2018243, 0.1811979, 0.8446119, // Expected quaternion
683  "[ 0.8528686 -0.1197639 0.5082046;"
684  " 0.4924039 0.5082046 -0.7065880;"
685  " -0.1736482 0.8528686 0.4924039 ]");
686  quat_vs_YPR(
687  -10.0_deg, -20.0_deg, -30.0_deg, // Input Yaw-pitch-roll
688  -0.2685358, -0.1448781, -0.1276794, 0.9437144, // Expected quaternion
689  "[ 0.9254166 0.3187958 -0.2048741;"
690  " -0.1631759 0.8231729 0.5438381;"
691  " 0.3420202 -0.4698463 0.8137977 ]");
692  quat_vs_YPR(
693  -179.9995949_deg, -90.0_deg, 0.0_deg, // Input Yaw-pitch-roll
694  0.7071068, 0, 0.7071068, -0.000005, // Expected quaternion
695  "[ 0.0000000 0.0000071 1.0000000;"
696  " -0.0000071 -1.0000000 0.0000071;"
697  " 1.0000000 -0.0000071 0.0000000 ]");
698 }
699 
700 TEST_F(Pose3DQuatTests, FromYPRAndBack)
701 {
702  test_fromYPRAndBack(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
703  test_fromYPRAndBack(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
704  test_fromYPRAndBack(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
705  test_fromYPRAndBack(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
706  test_fromYPRAndBack(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
707  test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
708  test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
709 }
710 
711 TEST_F(Pose3DQuatTests, UnaryInverse)
712 {
713  test_unaryInverse(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
714  test_unaryInverse(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
715  test_unaryInverse(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
716  test_unaryInverse(1.0, 2.0, 3.0, 179.0_deg, 0.0_deg, 60.0_deg);
717  test_unaryInverse(1.0, 2.0, 3.0, -179.0_deg, 0.0_deg, 60.0_deg);
718  test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, 89.0_deg, 0.0_deg);
719  test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
720 }
721 
722 TEST_F(Pose3DQuatTests, CopyOperator)
723 {
724  test_copy(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
725  test_copy(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
726  test_copy(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
727  test_copy(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
728 }
729 
730 TEST_F(Pose3DQuatTests, Compose)
731 {
732  test_compose(
733  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 2.0, -5.0, 8.0, 40.0_deg,
734  -5.0_deg, 25.0_deg);
735 
736  test_compose(
737  25.0, 2.0, 3.0, -30.0_deg, 90.0_deg, 0.0_deg, -10.0, 4.0, -8.0,
738  20.0_deg, 9.0_deg, 0.0_deg);
739 }
740 
741 TEST_F(Pose3DQuatTests, ComposeWithPoint)
742 {
743  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
744  test_composePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
745  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
746  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
747  test_composePoint(
748  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
749  test_composePoint(
750  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
751 }
752 
753 TEST_F(Pose3DQuatTests, ComposeWithPointJacob)
754 {
755  test_composePointJacob(
756  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
757  test_composePointJacob(
758  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
759  test_composePointJacob(
760  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
761  test_composePointJacob(
762  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
763  test_composePointJacob(
764  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
765  test_composePointJacob(
766  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
767 }
768 
769 TEST_F(Pose3DQuatTests, InvComposeWithPoint)
770 {
771  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
772  test_invComposePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
773  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
774  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
775  test_invComposePoint(
776  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
777  test_invComposePoint(
778  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
779 }
780 
781 TEST_F(Pose3DQuatTests, InvComposeWithPointJacob)
782 {
783  test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
784  test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 1, 2, 3);
785  test_invComposePointJacob(
786  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
787  test_invComposePointJacob(
788  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
789  test_invComposePointJacob(
790  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
791  test_invComposePointJacob(
792  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
793  test_invComposePointJacob(
794  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
795  test_invComposePointJacob(
796  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
797  test_invComposePointJacob(
798  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
799 }
800 
801 TEST_F(Pose3DQuatTests, ComposeInvComposePoint)
802 {
803  test_composeAndInvComposePoint(
804  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
805  test_composeAndInvComposePoint(
806  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
807  test_composeAndInvComposePoint(
808  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
809  test_composeAndInvComposePoint(
810  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
811  test_composeAndInvComposePoint(
812  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
813  test_composeAndInvComposePoint(
814  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
815 }
816 
817 TEST_F(Pose3DQuatTests, ComposePoint_vs_CPose3D)
818 {
819  test_composePoint_vs_CPose3D(
820  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
821  test_composePoint_vs_CPose3D(
822  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
823  test_composePoint_vs_CPose3D(
824  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
825  test_composePoint_vs_CPose3D(
826  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
827  test_composePoint_vs_CPose3D(
828  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
829  test_composePoint_vs_CPose3D(
830  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
831 }
832 
833 TEST_F(Pose3DQuatTests, InvComposePoint_vs_CPose3D)
834 {
835  test_invComposePoint_vs_CPose3D(
836  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
837  test_invComposePoint_vs_CPose3D(
838  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
839  test_invComposePoint_vs_CPose3D(
840  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
841  test_invComposePoint_vs_CPose3D(
842  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
843 
844  for (size_t i = 0; i < 10; i++)
845  {
846  std::vector<double> v(9);
847  mrpt::random::getRandomGenerator().drawGaussian1DVector(v, 0, 1);
848  test_invComposePoint_vs_CPose3D(
849  v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]);
850  }
851 }
852 
853 TEST_F(Pose3DQuatTests, SphericalCoordsJacobian)
854 {
855  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
856  test_sphericalCoords(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
857  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
858  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
859  test_sphericalCoords(
860  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
861  test_sphericalCoords(
862  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
863 }
864 
865 TEST_F(Pose3DQuatTests, NormalizationJacobian)
866 {
867  test_normalizeJacob(0.0_deg, 0.0_deg, 0.0_deg);
868  test_normalizeJacob(10.0_deg, 0.0_deg, 0.0_deg);
869  test_normalizeJacob(0.0_deg, 10.0_deg, 0.0_deg);
870  test_normalizeJacob(0.0_deg, 0.0_deg, 10.0_deg);
871  test_normalizeJacob(-30.0_deg, 10.0_deg, 60.0_deg);
872  test_normalizeJacob(10.0_deg, -50.0_deg, -40.0_deg);
873 }
quat_vs_YPR
static void quat_vs_YPR(double yaw, double pitch, double roll, double qx, double qy, double qz, double qw, const std::string &sRotMat)
Definition: CPose3DQuat_unittest.cpp:610
Pose3DQuatTests::test_sphericalCoords
void test_sphericalCoords(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:489
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
mrpt::math::CQuaternion::rotationMatrix
void rotationMatrix(MATRIXLIKE &M) const
Calculate the 3x3 rotation matrix associated to this quaternion: .
Definition: CQuaternion.h:380
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
Pose3DQuatTests::test_composePointJacob
void test_composePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:136
mrpt::math::CQuaternion::normalize
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:301
CPose3DQuat.h
mrpt::math::MatrixVectorBase::asString
std::string asString() const
Returns a string representation of the vector/matrix, using Eigen&#39;s default settings.
Definition: MatrixVectorBase_impl.h:337
Pose3DQuatTests::test_fromYPRAndBack
void test_fromYPRAndBack(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:340
mrpt::random::CRandomGenerator::drawGaussian1DVector
void drawGaussian1DVector(VEC &v, const double mean=0, const double std=1)
Fills the given vector with independent, 1D-normally distributed samples.
Definition: RandomGenerators.h:239
mrpt::math::TPoint3D_data::z
T z
Definition: TPoint3D.h:29
mrpt::math::CQuaternion::y
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
mrpt::poses::CPose3D::inverseComposePoint
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 .
Pose3DQuatTests::func_normalizeJacob
static void func_normalizeJacob(const CVectorFixedDouble< 4 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 4 > &Y)
Definition: CPose3DQuat_unittest.cpp:555
mrpt::math::CQuaternionDouble
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:540
Pose3DQuatTests::test_copy
void test_copy(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:392
std
STL namespace.
mrpt::poses::CPose3DQuat::sphericalCoordinates
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...
Definition: CPose3DQuat.cpp:386
CPose3D.h
Pose3DQuatTests::TearDown
void TearDown() override
Definition: CPose3DQuat_unittest.cpp:29
mrpt::poses::CPose3D::getAsQuaternion
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:501
mrpt::poses::CPose3DQuat::composePoint
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 .
Definition: CPose3DQuat.cpp:114
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::CQuaternion::r
T r() const
Return r (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
mrpt::poses::CPose3DQuat::inverseComposePoint
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 .
Definition: CPose3DQuat.cpp:187
TEST
TEST(QuatTests, Quat_vs_YPR)
Definition: CPose3DQuat_unittest.cpp:662
mrpt::math::MatrixVectorBase::block
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
Definition: MatrixVectorBase.h:138
mrpt::poses::CPose3D::sphericalCoordinates
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
Definition: CPose3D.cpp:312
Pose3DQuatTests::test_invComposePoint
void test_invComposePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:202
Pose3DQuatTests::test_composeAndInvComposePoint
void test_composeAndInvComposePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:415
eps
const double eps
Definition: distributions_unittest.cpp:21
mrpt::math::TPoint3D
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
mrpt::math::MatrixVectorBase::fromMatlabStringFormat
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...
Definition: MatrixVectorBase_impl.h:24
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::poses::CPose3DQuat
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
Pose3DQuatTests::func_compose_point
static void func_compose_point(const CVectorFixedDouble< 7+3 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:124
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:31
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::math::estimateJacobian
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
mrpt::math::CMatrixFixed::rows
constexpr size_type rows() const
Number of rows in the matrix.
Definition: CMatrixFixed.h:227
Pose3DQuatTests::test_invComposePointJacob
void test_invComposePointJacob(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:246
mrpt::math::TPoint3D_data::y
T y
Definition: TPoint3D.h:29
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
Pose3DQuatTests::test_compose
void test_compose(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
Definition: CPose3DQuat_unittest.cpp:30
mrpt::math::TPoint3D_data::x
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::math::CQuaternion::x
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:105
mrpt::math::CQuaternion::normalizationJacobian
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:313
Pose3DQuatTests::SetUp
void SetUp() override
Definition: CPose3DQuat_unittest.cpp:28
R
const float R
Definition: CKinematicChain.cpp:137
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Pose3DQuatTests::test_invComposePoint_vs_CPose3D
void test_invComposePoint_vs_CPose3D(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:444
mrpt::math::TPoint3D_< double >
num_jacobian.h
Pose3DQuatTests::test_normalizeJacob
void test_normalizeJacob(double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:565
EXPECT_EQ
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
mrpt::math::CMatrixFixed::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
Pose3DQuatTests::test_composePoint_vs_CPose3D
void test_composePoint_vs_CPose3D(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:429
mrpt::poses::CPose3D::composePoint
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...
mrpt::math::CMatrixFixed::cols
constexpr size_type cols() const
Number of columns in the matrix.
Definition: CMatrixFixed.h:230
mrpt::poses::CPoseOrPoint::getHomogeneousMatrixVal
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:278
Pose3DQuatTests::func_spherical_coords
static void func_spherical_coords(const CVectorFixedDouble< 7+3 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:478
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
mrpt::math::CQuaternion
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
mrpt::poses::CPose3D::getRotationMatrix
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
mrpt::math::CQuaternion::z
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
mrpt::math::CMatrixDynamic::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixDynamic.h:540
mrpt::math::CQuaternion::w
T w() const
Return w (real part) coordinate of the quaternion.
Definition: CQuaternion.h:103
Pose3DQuatTests::test_unaryInverse
void test_unaryInverse(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:368
Pose3DQuatTests::func_inv_compose_point
static void func_inv_compose_point(const CVectorFixedDouble< 7+3 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:232
Pose3DQuatTests::test_composePoint
void test_composePoint(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double x, double y, double z)
Definition: CPose3DQuat_unittest.cpp:99
TEST_F
TEST_F(Pose3DQuatTests, FromYPRAndBack)
Definition: CPose3DQuat_unittest.cpp:700
mrpt::poses::CPoseOrPoint::asVectorVal
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


Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020