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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST