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 {}
31  void test_compose(
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 
51  EXPECT_NEAR(
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 
61  EXPECT_NEAR(
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;
75  EXPECT_NEAR(
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;
84  EXPECT_NEAR(
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;
93  EXPECT_NEAR(
94  0,
95  (A.asVectorVal() - q1_c_q2.asVectorVal()).array().abs().sum(),
96  1e-6);
97  }
98  }
99 
100  void test_composePoint(
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,
127  CVectorFixedDouble<3>& Y)
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 
138  void test_composePointJacob(
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 
145  CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
146  CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
147 
148  TPoint3D l;
149  q1.composePoint(x, y, z, l.x, l.y, l.z, &df_dpoint, &df_dpose);
150 
151  // Numerical approximation:
152  CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
153  CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
154  {
155  CVectorFixedDouble<7 + 3> x_mean;
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;
162  CVectorFixedDouble<7 + 3> x_incrs;
163  x_incrs.fill(1e-7);
164  CMatrixDouble numJacobs;
165  mrpt::math::estimateJacobian(
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 
204  void test_invComposePoint(
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 
234  static void func_inv_compose_point(
235  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
236  CVectorFixedDouble<3>& Y)
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 
249  void test_invComposePointJacob(
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 
256  CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
257  CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
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:
287  CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
288  CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
289  {
290  CVectorFixedDouble<7 + 3> x_mean;
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;
297  CVectorFixedDouble<7 + 3> x_incrs;
298  x_incrs.fill(1e-7);
299  CMatrixDouble numJacobs;
300  mrpt::math::estimateJacobian(
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 
343  void test_fromYPRAndBack(
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,
353  (p1.getHomogeneousMatrixVal<CMatrixDouble44>() -
354  q1.getHomogeneousMatrixVal<CMatrixDouble44>())
355  .array()
356  .abs()
357  .sum(),
358  1e-5)
359  << "p1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
360  << p1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
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 
371  void test_unaryInverse(
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,
383  (p1_inv.getHomogeneousMatrixVal<CMatrixDouble44>() -
384  q1_inv.getHomogeneousMatrixVal<CMatrixDouble44>())
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,
406  (q1.getHomogeneousMatrixVal<CMatrixDouble44>() -
407  q2.getHomogeneousMatrixVal<CMatrixDouble44>())
408  .array()
409  .abs()
410  .sum(),
411  1e-5)
412  << "q1.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
413  << q1.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
414  << "q2.getHomogeneousMatrixVal<CMatrixDouble44>():\n"
415  << q2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl;
416  }
417 
418  void test_composeAndInvComposePoint(
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 
432  void test_composePoint_vs_CPose3D(
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 
447  void test_invComposePoint_vs_CPose3D(
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  {
462  CPose3DQuat q = mrpt::poses::CPose3DQuat(p1);
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 
481  static void func_spherical_coords(
482  const CVectorFixedDouble<7 + 3>& x, const double& dummy,
483  CVectorFixedDouble<3>& Y)
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 
493  void test_sphericalCoords(
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 
500  CMatrixFixed<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
501  CMatrixFixed<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
502 
503  double hr, hy, hp;
504  q1.sphericalCoordinates(p, hr, hy, hp, &df_dpoint, &df_dpose);
505 
506  // Numerical approximation:
507  CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
508  CMatrixFixed<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
509  {
510  CVectorFixedDouble<7 + 3> x_mean;
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;
517  CVectorFixedDouble<7 + 3> x_incrs;
518  x_incrs.fill(1e-7);
519  CMatrixDouble numJacobs;
520  mrpt::math::estimateJacobian(
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,
561  CVectorFixedDouble<4>& Y)
562  {
563  MRPT_UNUSED_PARAM(dummy);
564  CQuaternionDouble q;
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);
573  CQuaternionDouble q1;
574  pp.getAsQuaternion(q1);
575 
576  CMatrixFixed<double, 4, 4> df_dpose(UNINITIALIZED_MATRIX);
577  q1.normalizationJacobian(df_dpose);
578 
579  // Numerical approximation:
580  CMatrixFixed<double, 4, 4> num_df_dpose(UNINITIALIZED_MATRIX);
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;
589  mrpt::math::estimateJacobian(
590  x_mean,
591  std::function<void(
592  const CVectorFixedDouble<4>& x, const double& dummy,
593  CVectorFixedDouble<4>& Y)>(&func_normalizeJacob),
594  x_incrs, DUMMY, numJacobs);
595 
596  num_df_dpose = numJacobs.block<4, 4>(0, 0);
597  }
598 
599  // Compare:
600  EXPECT_NEAR(
601  0,
602  (df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
603  3e-3)
604  << "q1: " << q1 << endl
605  << "Numeric approximation of df_dpose: " << endl
606  << num_df_dpose.asEigen() << endl
607  << "Implemented method: " << endl
608  << df_dpose.asEigen() << endl
609  << "Error: " << endl
610  << df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
611  }
612 };
613 
614 TEST_F(Pose3DQuatTests, FromYPRAndBack)
615 {
616  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
617  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(90), DEG2RAD(0), DEG2RAD(0));
618  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
619  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(60));
620  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(60));
621  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(89), DEG2RAD(0));
622  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(-89), DEG2RAD(0));
623 }
624 
625 TEST_F(Pose3DQuatTests, UnaryInverse)
626 {
627  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
628  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(90), DEG2RAD(0), DEG2RAD(0));
629  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
630  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(60));
631  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(60));
632  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(89), DEG2RAD(0));
633  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(-89), DEG2RAD(0));
634 }
635 
636 TEST_F(Pose3DQuatTests, CopyOperator)
637 {
638  test_copy(1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
639  test_copy(1.0, 2.0, 3.0, DEG2RAD(90), DEG2RAD(0), DEG2RAD(0));
640  test_copy(1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
641  test_copy(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(-89), DEG2RAD(0));
642 }
643 
644 TEST_F(Pose3DQuatTests, Compose)
645 {
646  test_compose(
647  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 2.0, -5.0, 8.0,
648  DEG2RAD(40), DEG2RAD(-5), DEG2RAD(25));
649 
650  test_compose(
651  25.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(90), DEG2RAD(0), -10.0, 4.0, -8.0,
652  DEG2RAD(20), DEG2RAD(9), DEG2RAD(0));
653 }
654 
655 TEST_F(Pose3DQuatTests, ComposeWithPoint)
656 {
657  test_composePoint(
658  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
659  test_composePoint(
660  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
661  test_composePoint(
662  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
663  test_composePoint(
664  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
665  test_composePoint(
666  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
667  30.0);
668  test_composePoint(
669  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
670  8.0);
671 }
672 
673 TEST_F(Pose3DQuatTests, ComposeWithPointJacob)
674 {
675  test_composePointJacob(
676  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
677  test_composePointJacob(
678  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
679  test_composePointJacob(
680  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
681  test_composePointJacob(
682  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
683  test_composePointJacob(
684  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
685  30.0);
686  test_composePointJacob(
687  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
688  8.0);
689 }
690 
691 TEST_F(Pose3DQuatTests, InvComposeWithPoint)
692 {
693  test_invComposePoint(
694  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
695  test_invComposePoint(
696  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
697  test_invComposePoint(
698  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
699  test_invComposePoint(
700  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
701  test_invComposePoint(
702  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
703  30.0);
704  test_invComposePoint(
705  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
706  8.0);
707 }
708 
709 TEST_F(Pose3DQuatTests, InvComposeWithPointJacob)
710 {
711  test_invComposePointJacob(
712  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0, 0, 0);
713  test_invComposePointJacob(
714  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 1, 2, 3);
715  test_invComposePointJacob(
716  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0, 0, 0);
717  test_invComposePointJacob(
718  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
719  test_invComposePointJacob(
720  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
721  test_invComposePointJacob(
722  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
723  test_invComposePointJacob(
724  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
725  test_invComposePointJacob(
726  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
727  30.0);
728  test_invComposePointJacob(
729  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
730  8.0);
731 }
732 
733 TEST_F(Pose3DQuatTests, ComposeInvComposePoint)
734 {
735  test_composeAndInvComposePoint(
736  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
737  test_composeAndInvComposePoint(
738  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
739  test_composeAndInvComposePoint(
740  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
741  test_composeAndInvComposePoint(
742  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
743  test_composeAndInvComposePoint(
744  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
745  30.0);
746  test_composeAndInvComposePoint(
747  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
748  8.0);
749 }
750 
751 TEST_F(Pose3DQuatTests, ComposePoint_vs_CPose3D)
752 {
753  test_composePoint_vs_CPose3D(
754  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
755  test_composePoint_vs_CPose3D(
756  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
757  test_composePoint_vs_CPose3D(
758  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
759  test_composePoint_vs_CPose3D(
760  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
761  test_composePoint_vs_CPose3D(
762  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
763  30.0);
764  test_composePoint_vs_CPose3D(
765  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
766  8.0);
767 }
768 
769 TEST_F(Pose3DQuatTests, InvComposePoint_vs_CPose3D)
770 {
771  test_invComposePoint_vs_CPose3D(
772  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
773  test_invComposePoint_vs_CPose3D(
774  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
775  test_invComposePoint_vs_CPose3D(
776  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
777  test_invComposePoint_vs_CPose3D(
778  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
779 
780  for (size_t i = 0; i < 10; i++)
781  {
782  std::vector<double> v(9);
783  mrpt::random::getRandomGenerator().drawGaussian1DVector(v, 0, 1);
784  test_invComposePoint_vs_CPose3D(
785  v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]);
786  }
787 }
788 
789 TEST_F(Pose3DQuatTests, SphericalCoordsJacobian)
790 {
791  test_sphericalCoords(
792  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
793  test_sphericalCoords(
794  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
795  test_sphericalCoords(
796  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
797  test_sphericalCoords(
798  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
799  test_sphericalCoords(
800  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
801  30.0);
802  test_sphericalCoords(
803  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
804  8.0);
805 }
806 
807 TEST_F(Pose3DQuatTests, NormalizationJacobian)
808 {
809  test_normalizeJacob(DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
810  test_normalizeJacob(DEG2RAD(10), DEG2RAD(0), DEG2RAD(0));
811  test_normalizeJacob(DEG2RAD(0), DEG2RAD(10), DEG2RAD(0));
812  test_normalizeJacob(DEG2RAD(0), DEG2RAD(0), DEG2RAD(10));
813  test_normalizeJacob(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
814  test_normalizeJacob(DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40));
815 }
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:493
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
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:138
CPose3DQuat.h
z
GLdouble GLdouble z
Definition: glext.h:3879
mrpt::math::TPoint3D_data::x
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
mrpt::poses::CPose3D::composePoint
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:368
Pose3DQuatTests::test_fromYPRAndBack
void test_fromYPRAndBack(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:343
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:237
mrpt::math::CQuaternion::y
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:89
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:43
Pose3DQuatTests::func_spherical_coords
static void func_spherical_coords(const CVectorFixedDouble< 7+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:481
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
mrpt::math::TPoint3D_data::y
double y
Definition: TPoint3D.h:83
mrpt::math::CQuaternionDouble
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:505
Pose3DQuatTests::test_copy
void test_copy(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:395
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:559
CPose3D.h
Pose3DQuatTests::TearDown
void TearDown() override
Definition: CPose3DQuat_unittest.cpp:30
Pose3DQuatTests::func_compose_point
static void func_compose_point(const CVectorFixedDouble< 7+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:125
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::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:19
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 coordinate of the quaternion.
Definition: CQuaternion.h:85
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
mrpt::poses::CPose3D::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, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:647
mrpt::math::TPoint3D_data::z
double z
Definition: TPoint3D.h:83
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::getAsQuaternion
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixed< double, 4, 3 > *out_dq_dr=nullptr) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:508
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:303
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:204
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:418
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:234
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
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:249
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:31
v
const GLdouble * v
Definition: glext.h:3684
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:27
mrpt::math::CQuaternion::x
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:87
mrpt::math::CQuaternion::normalizationJacobian
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:286
Pose3DQuatTests::SetUp
void SetUp() override
Definition: CPose3DQuat_unittest.cpp:29
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
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:447
num_jacobian.h
Pose3DQuatTests::test_normalizeJacob
void test_normalizeJacob(double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:570
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:432
y
GLenum GLint GLint y
Definition: glext.h:3542
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
x
GLenum GLint x
Definition: glext.h:3542
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::math::TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:90
mrpt::math::CQuaternion::z
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:91
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:512
Pose3DQuatTests::test_unaryInverse
void test_unaryInverse(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:371
p
GLfloat GLfloat p
Definition: glext.h:6398
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:100
TEST_F
TEST_F(Pose3DQuatTests, FromYPRAndBack)
Definition: CPose3DQuat_unittest.cpp:614
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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019