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-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, const double& dummy,
126  CVectorFixedDouble<3>& Y)
127  {
128  MRPT_UNUSED_PARAM(dummy);
129  CPose3DQuat q(
130  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
131  q.quat().normalize();
132  const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
133  const CPoint3D pp = q + p;
134  for (int i = 0; i < 3; i++) Y[i] = pp[i];
135  }
136 
137  void test_composePointJacob(
138  double x1, double y1, double z1, double yaw1, double pitch1,
139  double roll1, double x, double y, double z)
140  {
141  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
142  const CPoint3D p(x, y, z);
143 
144  CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
145  CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
146 
147  TPoint3D l;
148  q1.composePoint(x, y, z, l.x, l.y, l.z, &df_dpoint, &df_dpose);
149 
150  // Numerical approximation:
151  CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
152  CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
153  {
154  CVectorFixedDouble<7 + 3> x_mean;
155  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
156  x_mean[7 + 0] = x;
157  x_mean[7 + 1] = y;
158  x_mean[7 + 2] = z;
159 
160  double DUMMY = 0;
161  CVectorFixedDouble<7 + 3> x_incrs;
162  x_incrs.fill(1e-7);
163  CMatrixDouble numJacobs;
164  mrpt::math::estimateJacobian(
165  x_mean,
166  std::function<void(
167  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
168  CVectorFixedDouble<3>& Y)>(&func_compose_point),
169  x_incrs, DUMMY, numJacobs);
170 
171  num_df_dpose = numJacobs.asEigen().block<3, 7>(0, 0);
172  num_df_dpoint = numJacobs.asEigen().block<3, 3>(0, 7);
173  }
174 
175  // Compare:
176  EXPECT_NEAR(
177  0,
178  (df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
179  3e-3)
180  << "q1: " << q1 << endl
181  << "p: " << p << endl
182  << "Numeric approximation of df_dpoint: " << endl
183  << num_df_dpoint.asEigen() << endl
184  << "Implemented method: " << endl
185  << df_dpoint << endl
186  << "Error: " << endl
187  << df_dpoint.asEigen() - num_df_dpoint.asEigen() << endl;
188 
189  EXPECT_NEAR(
190  0,
191  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
192  3e-3)
193  << "q1: " << q1 << endl
194  << "p: " << p << endl
195  << "Numeric approximation of df_dpose: " << endl
196  << num_df_dpose.asEigen() << endl
197  << "Implemented method: " << endl
198  << df_dpose.asEigen() << endl
199  << "Error: " << endl
200  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
201  }
202 
203  void test_invComposePoint(
204  double x1, double y1, double z1, double yaw1, double pitch1,
205  double roll1, double x, double y, double z)
206  {
207  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
208  const CPose3DQuat q1(p1);
209  const CPoint3D p(x, y, z);
210 
211  CPoint3D p_minus_p1 = p - p1;
212  CPoint3D p_minus_q1 = p - q1;
213 
214  CPoint3D p_rec = q1 + p_minus_q1;
215 
216  EXPECT_NEAR(
217  0,
218  (p_minus_p1.asVectorVal() - p_minus_q1.asVectorVal())
219  .array()
220  .abs()
221  .sum(),
222  1e-5)
223  << "p_minus_p1: " << p_minus_p1 << endl
224  << "p_minus_q1: " << p_minus_q1 << endl;
225 
226  EXPECT_NEAR(
227  0, (p_rec.asVectorVal() - p.asVectorVal()).array().abs().sum(),
228  1e-5)
229  << "p_rec: " << p_rec << endl
230  << "p: " << p << endl;
231  }
232 
233  static void func_inv_compose_point(
234  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
235  CVectorFixedDouble<3>& Y)
236  {
237  MRPT_UNUSED_PARAM(dummy);
238  CPose3DQuat q(
239  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
240  q.quat().normalize();
241  const CPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
242  const CPoint3D pp = p - q;
243  Y[0] = pp.x();
244  Y[1] = pp.y();
245  Y[2] = pp.z();
246  }
247 
248  void test_invComposePointJacob(
249  double x1, double y1, double z1, double yaw1, double pitch1,
250  double roll1, double x, double y, double z)
251  {
252  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
253  const CPoint3D p(x, y, z);
254 
255  CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
256  CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
257 
258  TPoint3D l;
259  q1.inverseComposePoint(x, y, z, l.x, l.y, l.z, &df_dpoint, &df_dpose);
260 
261  // Also check the returned point, not just the jacobian:
262  TPoint3D theorical;
263  {
264  const double qr = q1.quat().r();
265  const double qx = q1.quat().x();
266  const double qy = q1.quat().y();
267  const double qz = q1.quat().z();
268  const double Ax = x - x1; // ax - x;
269  const double Ay = y - y1; // ay - y;
270  const double Az = z - z1; // az - z;
271  theorical.x = Ax + 2 * (Ay) * (qr * qz + qx * qy) -
272  2 * (Az) * (qr * qy - qx * qz) -
273  2 * (square(qy) + square(qz)) * (Ax);
274  theorical.y = Ay - 2 * (Ax) * (qr * qz - qx * qy) +
275  2 * (Az) * (qr * qx + qy * qz) -
276  2 * (square(qx) + square(qz)) * (Ay);
277  theorical.z = Az + 2 * (Ax) * (qr * qy + qx * qz) -
278  2 * (Ay) * (qr * qx - qy * qz) -
279  2 * (square(qx) + square(qy)) * (Az);
280  }
281  EXPECT_NEAR(theorical.x, l.x, 1e-5);
282  EXPECT_NEAR(theorical.y, l.y, 1e-5);
283  EXPECT_NEAR(theorical.z, l.z, 1e-5);
284 
285  // Numerical approximation:
286  CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
287  CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
288  {
289  CVectorFixedDouble<7 + 3> x_mean;
290  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
291  x_mean[7 + 0] = x;
292  x_mean[7 + 1] = y;
293  x_mean[7 + 2] = z;
294 
295  double DUMMY = 0;
296  CVectorFixedDouble<7 + 3> x_incrs;
297  x_incrs.fill(1e-7);
298  CMatrixDouble numJacobs;
299  mrpt::math::estimateJacobian(
300  x_mean,
301  std::function<void(
302  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
303  CVectorFixedDouble<3>& Y)>(&func_inv_compose_point),
304  x_incrs, DUMMY, numJacobs);
305 
306  num_df_dpose = numJacobs.block<3, 7>(0, 0);
307  num_df_dpoint = numJacobs.block<3, 3>(0, 7);
308  }
309 
310  // Compare:
311  EXPECT_NEAR(
312  0,
313  (df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
314  3e-3)
315  << "q1: " << q1 << endl
316  << "from pose: " << CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
317  << "p: " << p << endl
318  << "local: " << l << endl
319  << "Numeric approximation of df_dpoint: " << endl
320  << num_df_dpoint.asEigen() << endl
321  << "Implemented method: " << endl
322  << df_dpoint.asEigen() << endl
323  << "Error: " << endl
324  << df_dpoint - num_df_dpoint << endl;
325 
326  EXPECT_NEAR(
327  0,
328  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
329  3e-3)
330  << "q1: " << q1 << endl
331  << "from pose: " << CPose3D(x1, y1, z1, yaw1, pitch1, roll1) << endl
332  << "p: " << p << endl
333  << "local: " << l << endl
334  << "Numeric approximation of df_dpose: " << endl
335  << num_df_dpose.asEigen() << endl
336  << "Implemented method: " << endl
337  << df_dpose.asEigen() << endl
338  << "Error: " << endl
339  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
340  }
341 
342  void test_fromYPRAndBack(
343  double x1, double y1, double z1, double yaw1, double pitch1,
344  double roll1)
345  {
346  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
347  const CPose3DQuat q1(p1);
348  const CPose3D p1r = CPose3D(q1);
349 
350  EXPECT_NEAR(
351  0,
352  (p1.getHomogeneousMatrixVal<CMatrixDouble44>() -
353  q1.getHomogeneousMatrixVal<CMatrixDouble44>())
354  .array()
355  .abs()
356  .sum(),
357  1e-5)
358  << "p1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
359  << p1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
360  << "q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
361  << q1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
362 
363  EXPECT_NEAR(
364  0, (p1.asVectorVal() - p1r.asVectorVal()).array().abs().sum(), 1e-5)
365  << "p1: " << p1 << endl
366  << "q1: " << q1 << endl
367  << "p1r: " << p1r << endl;
368  }
369 
370  void test_unaryInverse(
371  double x1, double y1, double z1, double yaw1, double pitch1,
372  double roll1)
373  {
374  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
375  const CPose3D p1_inv = -p1;
376 
377  const CPose3DQuat q1(p1);
378  const CPose3DQuat q1_inv = -q1;
379 
380  EXPECT_NEAR(
381  0,
382  (p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() -
383  q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>())
384  .array()
385  .abs()
386  .sum(),
387  1e-5)
388  << "p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
389  << p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
390  << "q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
391  << q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
392  }
393 
394  void test_copy(
395  double x1, double y1, double z1, double yaw1, double pitch1,
396  double roll1)
397  {
398  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
399 
400  const CPose3DQuat q1(p1);
401  const CPose3DQuat q2 = q1;
402 
403  EXPECT_NEAR(
404  0,
405  (q1.getHomogeneousMatrixVal<CMatrixDouble44>() -
406  q2.getHomogeneousMatrixVal<CMatrixDouble44>())
407  .array()
408  .abs()
409  .sum(),
410  1e-5)
411  << "q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
412  << q1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
413  << "q2.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
414  << q2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
415  }
416 
417  void test_composeAndInvComposePoint(
418  double x1, double y1, double z1, double yaw1, double pitch1,
419  double roll1, double x, double y, double z)
420  {
421  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
422  TPoint3D pp(x, y, z), aux;
423  q1.composePoint(x, y, z, pp.x, pp.y, pp.z);
424  q1.inverseComposePoint(pp.x, pp.y, pp.z, aux.x, aux.y, aux.z);
425 
426  EXPECT_NEAR(x, aux.x, 1e-7);
427  EXPECT_NEAR(y, aux.y, 1e-7);
428  EXPECT_NEAR(z, aux.z, 1e-7);
429  }
430 
431  void test_composePoint_vs_CPose3D(
432  double x1, double y1, double z1, double yaw1, double pitch1,
433  double roll1, double x, double y, double z)
434  {
435  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
436  const CPose3DQuat q1(p1);
437  TPoint3D pt1, pt2;
438  p1.composePoint(x, y, z, pt1.x, pt1.y, pt1.z);
439  q1.composePoint(x, y, z, pt2.x, pt2.y, pt2.z);
440 
441  EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
442  EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
443  EXPECT_NEAR(pt1.z, pt2.z, 1e-7);
444  }
445 
446  void test_invComposePoint_vs_CPose3D(
447  double x1, double y1, double z1, double yaw1, double pitch1,
448  double roll1, double x, double y, double z)
449  {
450  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
451  const CPose3DQuat q1(p1);
452  TPoint3D pt1, pt2;
453  p1.inverseComposePoint(x, y, z, pt1.x, pt1.y, pt1.z);
454  q1.inverseComposePoint(x, y, z, pt2.x, pt2.y, pt2.z);
455 
456  EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
457  EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
458  EXPECT_NEAR(pt1.z, pt2.z, 1e-7);
459 
460  {
461  CPose3DQuat q = mrpt::poses::CPose3DQuat(p1);
462 
463  double gx = x, gy = y, gz = z;
464 
465  double dist, yaw, pitch;
466  p1.sphericalCoordinates(TPoint3D(gx, gy, gz), dist, yaw, pitch);
467 
468  double lx, ly, lz;
469  p1.inverseComposePoint(gx, gy, gz, lx, ly, lz);
470 
471  double lx2, ly2, lz2;
472  q.inverseComposePoint(gx, gy, gz, lx2, ly2, lz2);
473 
474  EXPECT_NEAR(lx, lx2, 1e-7);
475  EXPECT_NEAR(ly, ly2, 1e-7);
476  EXPECT_NEAR(lz, lz2, 1e-7);
477  }
478  }
479 
480  static void func_spherical_coords(
481  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
482  CVectorFixedDouble<3>& Y)
483  {
484  MRPT_UNUSED_PARAM(dummy);
485  CPose3DQuat q(
486  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
487  q.quat().normalize();
488  const TPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
489  q.sphericalCoordinates(p, Y[0], Y[1], Y[2]);
490  }
491 
492  void test_sphericalCoords(
493  double x1, double y1, double z1, double yaw1, double pitch1,
494  double roll1, double x, double y, double z)
495  {
496  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
497  const TPoint3D p(x, y, z);
498 
499  CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
500  CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
501 
502  double hr, hy, hp;
503  q1.sphericalCoordinates(p, hr, hy, hp, &df_dpoint, &df_dpose);
504 
505  // Numerical approximation:
506  CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
507  CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
508  {
509  CVectorFixedDouble<7 + 3> x_mean;
510  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
511  x_mean[7 + 0] = x;
512  x_mean[7 + 1] = y;
513  x_mean[7 + 2] = z;
514 
515  double DUMMY = 0;
516  CVectorFixedDouble<7 + 3> x_incrs;
517  x_incrs.fill(1e-7);
518  CMatrixDouble numJacobs;
519  mrpt::math::estimateJacobian(
520  x_mean,
521  std::function<void(
522  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
523  CVectorFixedDouble<3>& Y)>(&func_spherical_coords),
524  x_incrs, DUMMY, numJacobs);
525 
526  num_df_dpose = numJacobs.block<3, 7>(0, 0);
527  num_df_dpoint = numJacobs.block<3, 3>(0, 7);
528  }
529 
530  // Compare:
531  EXPECT_NEAR(
532  0,
533  (df_dpoint.asEigen() - num_df_dpoint.asEigen()).array().abs().sum(),
534  3e-3)
535  << "q1: " << q1 << endl
536  << "p: " << p << endl
537  << "Numeric approximation of df_dpoint: " << endl
538  << num_df_dpoint.asEigen() << endl
539  << "Implemented method: " << endl
540  << df_dpoint.asEigen() << endl
541  << "Error: " << endl
542  << df_dpoint.asEigen() - num_df_dpoint.asEigen() << endl;
543 
544  EXPECT_NEAR(
545  0,
546  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
547  3e-3)
548  << "q1: " << q1 << endl
549  << "p: " << p << endl
550  << "Numeric approximation of df_dpose: " << endl
551  << num_df_dpose.asEigen() << endl
552  << "Implemented method: " << endl
553  << df_dpose.asEigen() << endl
554  << "Error: " << endl
555  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
556  }
557 
558  static void func_normalizeJacob(
559  const CVectorFixedDouble<4>& x, const double& dummy,
560  CVectorFixedDouble<4>& Y)
561  {
562  MRPT_UNUSED_PARAM(dummy);
563  CQuaternionDouble q;
564  for (int i = 0; i < 4; i++) q[i] = x[i];
565  q.normalize();
566  for (int i = 0; i < 4; i++) Y[i] = q[i];
567  }
568 
569  void test_normalizeJacob(double yaw1, double pitch1, double roll1)
570  {
571  const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
572  CQuaternionDouble q1;
573  pp.getAsQuaternion(q1);
574 
575  CMatrixFixed<double, 4, 4> df_dpose(UNINITIALIZED_MATRIX);
576  q1.normalizationJacobian(df_dpose);
577 
578  // Numerical approximation:
579  CMatrixFixed<double, 4, 4> num_df_dpose(UNINITIALIZED_MATRIX);
580  {
581  CVectorFixedDouble<4> x_mean;
582  for (int i = 0; i < 4; i++) x_mean[i] = q1[i];
583 
584  double DUMMY = 0;
585  CVectorFixedDouble<4> x_incrs;
586  x_incrs.fill(1e-5);
587  CMatrixDouble numJacobs;
588  mrpt::math::estimateJacobian(
589  x_mean,
590  std::function<void(
591  const CVectorFixedDouble<4>& x, const double& dummy,
592  CVectorFixedDouble<4>& Y)>(&func_normalizeJacob),
593  x_incrs, DUMMY, numJacobs);
594 
595  num_df_dpose = numJacobs.block<4, 4>(0, 0);
596  }
597 
598  // Compare:
599  EXPECT_NEAR(
600  0,
601  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
602  3e-3)
603  << "q1: " << q1 << endl
604  << "Numeric approximation of df_dpose: " << endl
605  << num_df_dpose.asEigen() << endl
606  << "Implemented method: " << endl
607  << df_dpose.asEigen() << endl
608  << "Error: " << endl
609  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
610  }
611 };
612 
613 // Check yaw-pitch-roll [rad] vs its [qx,qy,qz,qw] representation:
614 static void quat_vs_YPR(
615  double yaw, double pitch, double roll, double qx, double qy, double qz,
616  double qw, const std::string& sRotMat)
617 {
618  const double eps = 1e-4;
619 
620  // First, test Yaw-pitch-roll to Rot matrix:
621  const mrpt::poses::CPose3D p(0, 0, 0, yaw, pitch, roll);
622  mrpt::math::CMatrixDouble33 R_gt;
623  std::stringstream sErr;
624  if (!R_gt.fromMatlabStringFormat(sRotMat, sErr))
625  GTEST_FAIL() << "Incorrect R_gt matrix: '" << sRotMat
626  << "'. Error: " << sErr.str() << "\n";
627  EXPECT_EQ(R_gt.cols(), 3) << " for: sRotMat='" << sRotMat << "'\n";
628  EXPECT_EQ(R_gt.rows(), 3) << " for: sRotMat='" << sRotMat << "'\n";
629 
630  EXPECT_NEAR(
631  0,
632  (R_gt.asEigen() - p.getRotationMatrix().asEigen())
633  .array()
634  .abs()
635  .maxCoeff(),
636  eps)
637  << "R_gt=\n"
638  << R_gt << "\np.R=\n"
639  << p.getRotationMatrix() << std::endl;
640 
641  // Convert to quat:
642  const auto q_gt = mrpt::math::CQuaternionDouble(qw, qx, qy, qz);
643  mrpt::math::CQuaternionDouble q;
644  p.getAsQuaternion(q);
645 
646  if (std::abs(q.w() - q_gt.w()) > eps || std::abs(q.x() - q_gt.x()) > eps ||
647  std::abs(q.y() - q_gt.y()) > eps || std::abs(q.z() - q_gt.z()) > eps)
648  {
649  GTEST_FAIL() << "q = " << q.asString()
650  << "\nExpected = " << q_gt.asString() << "\n";
651  }
652 
653  auto R = q.rotationMatrix<mrpt::math::CMatrixDouble33>();
654  EXPECT_NEAR(
655  0,
656  (R.asEigen() - p.getRotationMatrix().asEigen())
657  .array()
658  .abs()
659  .maxCoeff(),
660  eps)
661  << "q.R=\n"
662  << R << "\np.R=" << p.getRotationMatrix() << std::endl;
663 }
664 
665 // Check yaw-pitch-roll vs its [qx,qy,qz,qw] representation:
666 TEST(QuatTests, Quat_vs_YPR)
667 {
668  // Ground truth values obtained from:
669  // https://www.andre-gaschler.com/rotationconverter/
670  // Using: ZYX Euler angles convention
671 
672  quat_vs_YPR(
673  0.0_deg, 0.0_deg, 0.0_deg, // Input Yaw-pitch-roll
674  0, 0, 0, 1, // Expected quaternion
675  "[ 1.0000000 0.0000000 0.0000000; "
676  " 0.0000000 1.0000000 0.0000000; "
677  " 0.0000000 0.0000000 1.0000000 ]");
678  quat_vs_YPR(
679  90.0_deg, 0.0_deg, 0.0_deg, // Input Yaw-pitch-roll
680  0, 0, 0.7071068, 0.7071068, // Expected quaternion
681  "[ 0.0000000 -1.0000000 0.0000000;"
682  " 1.0000000 0.0000000 0.0000000;"
683  " 0.0000000 0.0000000 1.0000000 ]");
684  quat_vs_YPR(
685  30.0_deg, 10.0_deg, 60.0_deg, // Input Yaw-pitch-roll
686  0.4615897, 0.2018243, 0.1811979, 0.8446119, // Expected quaternion
687  "[ 0.8528686 -0.1197639 0.5082046;"
688  " 0.4924039 0.5082046 -0.7065880;"
689  " -0.1736482 0.8528686 0.4924039 ]");
690  quat_vs_YPR(
691  -10.0_deg, -20.0_deg, -30.0_deg, // Input Yaw-pitch-roll
692  -0.2685358, -0.1448781, -0.1276794, 0.9437144, // Expected quaternion
693  "[ 0.9254166 0.3187958 -0.2048741;"
694  " -0.1631759 0.8231729 0.5438381;"
695  " 0.3420202 -0.4698463 0.8137977 ]");
696  quat_vs_YPR(
697  -179.9995949_deg, -90.0_deg, 0.0_deg, // Input Yaw-pitch-roll
698  0.7071068, 0, 0.7071068, -0.000005, // Expected quaternion
699  "[ 0.0000000 0.0000071 1.0000000;"
700  " -0.0000071 -1.0000000 0.0000071;"
701  " 1.0000000 -0.0000071 0.0000000 ]");
702 }
703 
704 TEST_F(Pose3DQuatTests, FromYPRAndBack)
705 {
706  test_fromYPRAndBack(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
707  test_fromYPRAndBack(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
708  test_fromYPRAndBack(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
709  test_fromYPRAndBack(1.0, 2.0, 3.0, 179.0_deg, 0.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, 30.0_deg, 89.0_deg, 0.0_deg);
712  test_fromYPRAndBack(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
713 }
714 
715 TEST_F(Pose3DQuatTests, UnaryInverse)
716 {
717  test_unaryInverse(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
718  test_unaryInverse(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
719  test_unaryInverse(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
720  test_unaryInverse(1.0, 2.0, 3.0, 179.0_deg, 0.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, 30.0_deg, 89.0_deg, 0.0_deg);
723  test_unaryInverse(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
724 }
725 
726 TEST_F(Pose3DQuatTests, CopyOperator)
727 {
728  test_copy(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg);
729  test_copy(1.0, 2.0, 3.0, 90.0_deg, 0.0_deg, 0.0_deg);
730  test_copy(1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg);
731  test_copy(1.0, 2.0, 3.0, 30.0_deg, -89.0_deg, 0.0_deg);
732 }
733 
734 TEST_F(Pose3DQuatTests, Compose)
735 {
736  test_compose(
737  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 2.0, -5.0, 8.0, 40.0_deg,
738  -5.0_deg, 25.0_deg);
739 
740  test_compose(
741  25.0, 2.0, 3.0, -30.0_deg, 90.0_deg, 0.0_deg, -10.0, 4.0, -8.0,
742  20.0_deg, 9.0_deg, 0.0_deg);
743 }
744 
745 TEST_F(Pose3DQuatTests, ComposeWithPoint)
746 {
747  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
748  test_composePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
749  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
750  test_composePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
751  test_composePoint(
752  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
753  test_composePoint(
754  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
755 }
756 
757 TEST_F(Pose3DQuatTests, ComposeWithPointJacob)
758 {
759  test_composePointJacob(
760  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
761  test_composePointJacob(
762  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
763  test_composePointJacob(
764  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
765  test_composePointJacob(
766  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
767  test_composePointJacob(
768  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
769  test_composePointJacob(
770  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
771 }
772 
773 TEST_F(Pose3DQuatTests, InvComposeWithPoint)
774 {
775  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
776  test_invComposePoint(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
777  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
778  test_invComposePoint(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
779  test_invComposePoint(
780  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
781  test_invComposePoint(
782  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
783 }
784 
785 TEST_F(Pose3DQuatTests, InvComposeWithPointJacob)
786 {
787  test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
788  test_invComposePointJacob(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 1, 2, 3);
789  test_invComposePointJacob(
790  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0);
791  test_invComposePointJacob(
792  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
793  test_invComposePointJacob(
794  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
795  test_invComposePointJacob(
796  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
797  test_invComposePointJacob(
798  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
799  test_invComposePointJacob(
800  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
801  test_invComposePointJacob(
802  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
803 }
804 
805 TEST_F(Pose3DQuatTests, ComposeInvComposePoint)
806 {
807  test_composeAndInvComposePoint(
808  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
809  test_composeAndInvComposePoint(
810  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
811  test_composeAndInvComposePoint(
812  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
813  test_composeAndInvComposePoint(
814  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
815  test_composeAndInvComposePoint(
816  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
817  test_composeAndInvComposePoint(
818  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
819 }
820 
821 TEST_F(Pose3DQuatTests, ComposePoint_vs_CPose3D)
822 {
823  test_composePoint_vs_CPose3D(
824  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
825  test_composePoint_vs_CPose3D(
826  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
827  test_composePoint_vs_CPose3D(
828  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
829  test_composePoint_vs_CPose3D(
830  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
831  test_composePoint_vs_CPose3D(
832  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
833  test_composePoint_vs_CPose3D(
834  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
835 }
836 
837 TEST_F(Pose3DQuatTests, InvComposePoint_vs_CPose3D)
838 {
839  test_invComposePoint_vs_CPose3D(
840  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
841  test_invComposePoint_vs_CPose3D(
842  1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
843  test_invComposePoint_vs_CPose3D(
844  1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
845  test_invComposePoint_vs_CPose3D(
846  1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
847 
848  for (size_t i = 0; i < 10; i++)
849  {
850  std::vector<double> v(9);
851  mrpt::random::getRandomGenerator().drawGaussian1DVector(v, 0, 1);
852  test_invComposePoint_vs_CPose3D(
853  v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]);
854  }
855 }
856 
857 TEST_F(Pose3DQuatTests, SphericalCoordsJacobian)
858 {
859  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
860  test_sphericalCoords(1.0, 2.0, 3.0, 10.0_deg, 0.0_deg, 0.0_deg, 10, 11, 12);
861  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 10.0_deg, 0.0_deg, 10, 11, 12);
862  test_sphericalCoords(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 10.0_deg, 10, 11, 12);
863  test_sphericalCoords(
864  1.0, 2.0, 3.0, -30.0_deg, 10.0_deg, 60.0_deg, 10.0, 20.0, 30.0);
865  test_sphericalCoords(
866  1.0, 2.0, 3.0, 10.0_deg, -50.0_deg, -40.0_deg, -5.0, -15.0, 8.0);
867 }
868 
869 TEST_F(Pose3DQuatTests, NormalizationJacobian)
870 {
871  test_normalizeJacob(0.0_deg, 0.0_deg, 0.0_deg);
872  test_normalizeJacob(10.0_deg, 0.0_deg, 0.0_deg);
873  test_normalizeJacob(0.0_deg, 10.0_deg, 0.0_deg);
874  test_normalizeJacob(0.0_deg, 0.0_deg, 10.0_deg);
875  test_normalizeJacob(-30.0_deg, 10.0_deg, 60.0_deg);
876  test_normalizeJacob(10.0_deg, -50.0_deg, -40.0_deg);
877 }
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:614
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:492
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:137
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:342
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
Pose3DQuatTests::func_spherical_coords
static void func_spherical_coords(const CVectorFixedDouble< 7+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:480
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 .
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:394
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
Pose3DQuatTests::func_normalizeJacob
static void func_normalizeJacob(const CVectorFixedDouble< 4 > &x, const double &dummy, CVectorFixedDouble< 4 > &Y)
Definition: CPose3DQuat_unittest.cpp:558
CPose3D.h
Pose3DQuatTests::TearDown
void TearDown() override
Definition: CPose3DQuat_unittest.cpp:29
Pose3DQuatTests::func_compose_point
static void func_compose_point(const CVectorFixedDouble< 7+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:124
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:517
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:666
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:203
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:417
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_inv_compose_point
static void func_inv_compose_point(const CVectorFixedDouble< 7+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:233
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:248
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:138
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:446
mrpt::math::TPoint3D_< double >
num_jacobian.h
Pose3DQuatTests::test_normalizeJacob
void test_normalizeJacob(double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:569
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:431
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
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:370
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:704
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
MRPT_UNUSED_PARAM
#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: 3a26b90fd Wed Mar 25 20:17:03 2020 +0100 at mié mar 25 23:05:41 CET 2020