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-2017, 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/jacobians.h>
13 #include <mrpt/random.h>
14 #include <mrpt/utils/CTraitsTest.h>
15 #include <gtest/gtest.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::poses;
19 using namespace mrpt::utils;
20 using namespace mrpt::math;
21 using namespace std;
22 
23 template class mrpt::utils::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::jacobians::jacob_numeric_estimate(
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::jacobians::jacob_numeric_estimate(
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() - q1.getHomogeneousMatrixVal())
339  .array()
340  .abs()
341  .sum(),
342  1e-5)
343  << "p1.getHomogeneousMatrixVal():\n"
344  << p1.getHomogeneousMatrixVal() << endl
345  << "q1.getHomogeneousMatrixVal():\n"
346  << q1.getHomogeneousMatrixVal() << endl;
347 
348  EXPECT_NEAR(
349  0, (p1.getAsVectorVal() - p1r.getAsVectorVal()).array().abs().sum(),
350  1e-5)
351  << "p1: " << p1 << endl
352  << "q1: " << q1 << endl
353  << "p1r: " << p1r << endl;
354  }
355 
356  void test_unaryInverse(
357  double x1, double y1, double z1, double yaw1, double pitch1,
358  double roll1)
359  {
360  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
361  const CPose3D p1_inv = -p1;
362 
363  const CPose3DQuat q1(p1);
364  const CPose3DQuat q1_inv = -q1;
365 
366  EXPECT_NEAR(
367  0, (p1_inv.getHomogeneousMatrixVal() -
368  q1_inv.getHomogeneousMatrixVal())
369  .array()
370  .abs()
371  .sum(),
372  1e-5)
373  << "p1_inv.getHomogeneousMatrixVal():\n"
374  << p1_inv.getHomogeneousMatrixVal() << endl
375  << "q1_inv.getHomogeneousMatrixVal():\n"
376  << q1_inv.getHomogeneousMatrixVal() << endl;
377  }
378 
379  void test_copy(
380  double x1, double y1, double z1, double yaw1, double pitch1,
381  double roll1)
382  {
383  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
384 
385  const CPose3DQuat q1(p1);
386  const CPose3DQuat q2 = q1;
387 
388  EXPECT_NEAR(
389  0, (q1.getHomogeneousMatrixVal() - q2.getHomogeneousMatrixVal())
390  .array()
391  .abs()
392  .sum(),
393  1e-5)
394  << "q1.getHomogeneousMatrixVal():\n"
395  << q1.getHomogeneousMatrixVal() << endl
396  << "q2.getHomogeneousMatrixVal():\n"
397  << q2.getHomogeneousMatrixVal() << endl;
398  }
399 
400  void test_composeAndInvComposePoint(
401  double x1, double y1, double z1, double yaw1, double pitch1,
402  double roll1, double x, double y, double z)
403  {
404  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
405  TPoint3D pp(x, y, z), aux;
406  q1.composePoint(x, y, z, pp.x, pp.y, pp.z);
407  q1.inverseComposePoint(pp.x, pp.y, pp.z, aux.x, aux.y, aux.z);
408 
409  EXPECT_NEAR(x, aux.x, 1e-7);
410  EXPECT_NEAR(y, aux.y, 1e-7);
411  EXPECT_NEAR(z, aux.z, 1e-7);
412  }
413 
414  void test_composePoint_vs_CPose3D(
415  double x1, double y1, double z1, double yaw1, double pitch1,
416  double roll1, double x, double y, double z)
417  {
418  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
419  const CPose3DQuat q1(p1);
420  TPoint3D pt1, pt2;
421  p1.composePoint(x, y, z, pt1.x, pt1.y, pt1.z);
422  q1.composePoint(x, y, z, pt2.x, pt2.y, pt2.z);
423 
424  EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
425  EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
426  EXPECT_NEAR(pt1.z, pt2.z, 1e-7);
427  }
428 
429  void test_invComposePoint_vs_CPose3D(
430  double x1, double y1, double z1, double yaw1, double pitch1,
431  double roll1, double x, double y, double z)
432  {
433  const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
434  const CPose3DQuat q1(p1);
435  TPoint3D pt1, pt2;
436  p1.inverseComposePoint(x, y, z, pt1.x, pt1.y, pt1.z);
437  q1.inverseComposePoint(x, y, z, pt2.x, pt2.y, pt2.z);
438 
439  EXPECT_NEAR(pt1.x, pt2.x, 1e-7);
440  EXPECT_NEAR(pt1.y, pt2.y, 1e-7);
441  EXPECT_NEAR(pt1.z, pt2.z, 1e-7);
442 
443  {
444  CPose3DQuat q = mrpt::poses::CPose3DQuat(p1);
445 
446  float gx = x, gy = y, gz = z;
447 
448  double dist, yaw, pitch;
449  p1.sphericalCoordinates(TPoint3D(gx, gy, gz), dist, yaw, pitch);
450 
451  double lx, ly, lz;
452  p1.inverseComposePoint(gx, gy, gz, lx, ly, lz);
453 
454  double lx2, ly2, lz2;
455  q.inverseComposePoint(gx, gy, gz, lx2, ly2, lz2);
456 
457  EXPECT_NEAR(lx, lx2, 1e-7);
458  EXPECT_NEAR(ly, ly2, 1e-7);
459  EXPECT_NEAR(lz, lz2, 1e-7);
460  }
461  }
462 
463  static void func_spherical_coords(
464  const CArrayDouble<7 + 3>& x, const double& dummy, CArrayDouble<3>& Y)
465  {
466  MRPT_UNUSED_PARAM(dummy);
467  CPose3DQuat q(
468  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
469  q.quat().normalize();
470  const TPoint3D p(x[7 + 0], x[7 + 1], x[7 + 2]);
471  q.sphericalCoordinates(p, Y[0], Y[1], Y[2]);
472  }
473 
474  void test_sphericalCoords(
475  double x1, double y1, double z1, double yaw1, double pitch1,
476  double roll1, double x, double y, double z)
477  {
478  const CPose3DQuat q1(CPose3D(x1, y1, z1, yaw1, pitch1, roll1));
479  const TPoint3D p(x, y, z);
480 
481  CMatrixFixedNumeric<double, 3, 3> df_dpoint(UNINITIALIZED_MATRIX);
482  CMatrixFixedNumeric<double, 3, 7> df_dpose(UNINITIALIZED_MATRIX);
483 
484  double hr, hy, hp;
485  q1.sphericalCoordinates(p, hr, hy, hp, &df_dpoint, &df_dpose);
486 
487  // Numerical approximation:
488  CMatrixFixedNumeric<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
489  CMatrixFixedNumeric<double, 3, 7> num_df_dpose(UNINITIALIZED_MATRIX);
490  {
491  CArrayDouble<7 + 3> x_mean;
492  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
493  x_mean[7 + 0] = x;
494  x_mean[7 + 1] = y;
495  x_mean[7 + 2] = z;
496 
497  double DUMMY = 0;
498  CArrayDouble<7 + 3> x_incrs;
499  x_incrs.assign(1e-7);
500  CMatrixDouble numJacobs;
501  mrpt::math::jacobians::jacob_numeric_estimate(
502  x_mean, std::function<void(
503  const CArrayDouble<7 + 3>& x, const double& dummy,
504  CArrayDouble<3>& Y)>(&func_spherical_coords),
505  x_incrs, DUMMY, numJacobs);
506 
507  numJacobs.extractMatrix(0, 0, num_df_dpose);
508  numJacobs.extractMatrix(0, 7, num_df_dpoint);
509  }
510 
511  // Compare:
512  EXPECT_NEAR(0, (df_dpoint - num_df_dpoint).array().abs().sum(), 3e-3)
513  << "q1: " << q1 << endl
514  << "p: " << p << endl
515  << "Numeric approximation of df_dpoint: " << endl
516  << num_df_dpoint << endl
517  << "Implemented method: " << endl
518  << df_dpoint << endl
519  << "Error: " << endl
520  << df_dpoint - num_df_dpoint << endl;
521 
522  EXPECT_NEAR(0, (df_dpose - num_df_dpose).array().abs().sum(), 3e-3)
523  << "q1: " << q1 << endl
524  << "p: " << p << endl
525  << "Numeric approximation of df_dpose: " << endl
526  << num_df_dpose << endl
527  << "Implemented method: " << endl
528  << df_dpose << endl
529  << "Error: " << endl
530  << df_dpose - num_df_dpose << endl;
531  }
532 
533  static void func_normalizeJacob(
534  const CArrayDouble<4>& x, const double& dummy, CArrayDouble<4>& Y)
535  {
536  MRPT_UNUSED_PARAM(dummy);
537  CQuaternionDouble q;
538  for (int i = 0; i < 4; i++) q[i] = x[i];
539  q.normalize();
540  for (int i = 0; i < 4; i++) Y[i] = q[i];
541  }
542 
543  void test_normalizeJacob(double yaw1, double pitch1, double roll1)
544  {
545  const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
546  CQuaternionDouble q1;
547  pp.getAsQuaternion(q1);
548 
549  CMatrixFixedNumeric<double, 4, 4> df_dpose(UNINITIALIZED_MATRIX);
550  q1.normalizationJacobian(df_dpose);
551 
552  // Numerical approximation:
553  CMatrixFixedNumeric<double, 4, 4> num_df_dpose(UNINITIALIZED_MATRIX);
554  {
555  CArrayDouble<4> x_mean;
556  for (int i = 0; i < 4; i++) x_mean[i] = q1[i];
557 
558  double DUMMY = 0;
559  CArrayDouble<4> x_incrs;
560  x_incrs.assign(1e-5);
561  CMatrixDouble numJacobs;
562  mrpt::math::jacobians::jacob_numeric_estimate(
563  x_mean, std::function<void(
564  const CArrayDouble<4>& x, const double& dummy,
565  CArrayDouble<4>& Y)>(&func_normalizeJacob),
566  x_incrs, DUMMY, numJacobs);
567 
568  numJacobs.extractMatrix(0, 0, num_df_dpose);
569  }
570 
571  // Compare:
572  EXPECT_NEAR(0, (df_dpose - num_df_dpose).array().abs().sum(), 3e-3)
573  << "q1: " << q1 << endl
574  << "Numeric approximation of df_dpose: " << endl
575  << num_df_dpose << endl
576  << "Implemented method: " << endl
577  << df_dpose << endl
578  << "Error: " << endl
579  << df_dpose - num_df_dpose << endl;
580  }
581 };
582 
583 TEST_F(Pose3DQuatTests, FromYPRAndBack)
584 {
585  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
586  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(90), DEG2RAD(0), DEG2RAD(0));
587  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
588  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(60));
589  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(60));
590  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(89), DEG2RAD(0));
591  test_fromYPRAndBack(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(-89), DEG2RAD(0));
592 }
593 
594 TEST_F(Pose3DQuatTests, UnaryInverse)
595 {
596  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
597  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(90), DEG2RAD(0), DEG2RAD(0));
598  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
599  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(179), DEG2RAD(0), DEG2RAD(60));
600  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(-179), DEG2RAD(0), DEG2RAD(60));
601  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(89), DEG2RAD(0));
602  test_unaryInverse(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(-89), DEG2RAD(0));
603 }
604 
605 TEST_F(Pose3DQuatTests, CopyOperator)
606 {
607  test_copy(1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
608  test_copy(1.0, 2.0, 3.0, DEG2RAD(90), DEG2RAD(0), DEG2RAD(0));
609  test_copy(1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
610  test_copy(1.0, 2.0, 3.0, DEG2RAD(30), DEG2RAD(-89), DEG2RAD(0));
611 }
612 
613 TEST_F(Pose3DQuatTests, Compose)
614 {
615  test_compose(
616  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 2.0, -5.0, 8.0,
617  DEG2RAD(40), DEG2RAD(-5), DEG2RAD(25));
618 
619  test_compose(
620  25.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(90), DEG2RAD(0), -10.0, 4.0, -8.0,
621  DEG2RAD(20), DEG2RAD(9), DEG2RAD(0));
622 }
623 
624 TEST_F(Pose3DQuatTests, ComposeWithPoint)
625 {
626  test_composePoint(
627  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
628  test_composePoint(
629  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
630  test_composePoint(
631  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
632  test_composePoint(
633  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
634  test_composePoint(
635  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
636  30.0);
637  test_composePoint(
638  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
639  8.0);
640 }
641 
642 TEST_F(Pose3DQuatTests, ComposeWithPointJacob)
643 {
644  test_composePointJacob(
645  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
646  test_composePointJacob(
647  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
648  test_composePointJacob(
649  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
650  test_composePointJacob(
651  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
652  test_composePointJacob(
653  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
654  30.0);
655  test_composePointJacob(
656  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
657  8.0);
658 }
659 
660 TEST_F(Pose3DQuatTests, InvComposeWithPoint)
661 {
662  test_invComposePoint(
663  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
664  test_invComposePoint(
665  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
666  test_invComposePoint(
667  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
668  test_invComposePoint(
669  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
670  test_invComposePoint(
671  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
672  30.0);
673  test_invComposePoint(
674  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
675  8.0);
676 }
677 
678 TEST_F(Pose3DQuatTests, InvComposeWithPointJacob)
679 {
680  test_invComposePointJacob(
681  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0, 0, 0);
682  test_invComposePointJacob(
683  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 1, 2, 3);
684  test_invComposePointJacob(
685  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0, 0, 0);
686  test_invComposePointJacob(
687  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
688  test_invComposePointJacob(
689  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
690  test_invComposePointJacob(
691  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
692  test_invComposePointJacob(
693  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
694  test_invComposePointJacob(
695  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
696  30.0);
697  test_invComposePointJacob(
698  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
699  8.0);
700 }
701 
702 TEST_F(Pose3DQuatTests, ComposeInvComposePoint)
703 {
704  test_composeAndInvComposePoint(
705  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
706  test_composeAndInvComposePoint(
707  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
708  test_composeAndInvComposePoint(
709  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
710  test_composeAndInvComposePoint(
711  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
712  test_composeAndInvComposePoint(
713  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
714  30.0);
715  test_composeAndInvComposePoint(
716  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
717  8.0);
718 }
719 
720 TEST_F(Pose3DQuatTests, ComposePoint_vs_CPose3D)
721 {
722  test_composePoint_vs_CPose3D(
723  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
724  test_composePoint_vs_CPose3D(
725  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
726  test_composePoint_vs_CPose3D(
727  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
728  test_composePoint_vs_CPose3D(
729  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
730  test_composePoint_vs_CPose3D(
731  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
732  30.0);
733  test_composePoint_vs_CPose3D(
734  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
735  8.0);
736 }
737 
738 TEST_F(Pose3DQuatTests, InvComposePoint_vs_CPose3D)
739 {
740  test_invComposePoint_vs_CPose3D(
741  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
742  test_invComposePoint_vs_CPose3D(
743  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
744  test_invComposePoint_vs_CPose3D(
745  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
746  test_invComposePoint_vs_CPose3D(
747  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
748 
749  for (size_t i = 0; i < 10; i++)
750  {
751  std::vector<double> v(9);
752  mrpt::random::getRandomGenerator().drawGaussian1DVector(v, 0, 1);
753  test_invComposePoint_vs_CPose3D(
754  v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]);
755  }
756 }
757 
758 TEST_F(Pose3DQuatTests, SphericalCoordsJacobian)
759 {
760  test_sphericalCoords(
761  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
762  test_sphericalCoords(
763  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 10, 11, 12);
764  test_sphericalCoords(
765  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 10, 11, 12);
766  test_sphericalCoords(
767  1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 10, 11, 12);
768  test_sphericalCoords(
769  1.0, 2.0, 3.0, DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60), 10.0, 20.0,
770  30.0);
771  test_sphericalCoords(
772  1.0, 2.0, 3.0, DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40), -5.0, -15.0,
773  8.0);
774 }
775 
776 TEST_F(Pose3DQuatTests, NormalizationJacobian)
777 {
778  test_normalizeJacob(DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
779  test_normalizeJacob(DEG2RAD(10), DEG2RAD(0), DEG2RAD(0));
780  test_normalizeJacob(DEG2RAD(0), DEG2RAD(10), DEG2RAD(0));
781  test_normalizeJacob(DEG2RAD(0), DEG2RAD(0), DEG2RAD(10));
782  test_normalizeJacob(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
783  test_normalizeJacob(DEG2RAD(10), DEG2RAD(-50), DEG2RAD(-40));
784 }
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:474
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:60
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:135
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:188
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
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
CPose3DQuat.h
z
GLdouble GLdouble z
Definition: glext.h:3872
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:265
Pose3DQuatTests::test_fromYPRAndBack
void test_fromYPRAndBack(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:329
Pose3DQuatTests::TearDown
virtual void TearDown()
Definition: CPose3DQuat_unittest.cpp:29
z2
INT32 z2
Definition: jidctint.cpp:130
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:189
Pose3DQuatTests::func_normalizeJacob
static void func_normalizeJacob(const CArrayDouble< 4 > &x, const double &dummy, CArrayDouble< 4 > &Y)
Definition: CPose3DQuat_unittest.cpp:533
mrpt::math::CQuaternion::y
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:91
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
mrpt::math::CQuaternionDouble
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:504
Pose3DQuatTests::test_copy
void test_copy(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:379
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:584
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:366
std
STL namespace.
CPose3D.h
mrpt::poses::CPoseOrPoint::getHomogeneousMatrixVal
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:276
mrpt::math::jacobians::jacob_numeric_estimate
void jacob_numeric_estimate(const VECTORLIKE &x, std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::m...
Definition: jacobians.h:144
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:55
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:113
Pose3DQuatTests::func_spherical_coords
static void func_spherical_coords(const CArrayDouble< 7+3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
Definition: CPose3DQuat_unittest.cpp:463
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::math::CQuaternion::r
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:87
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: CColouredPointsMap.h:366
mrpt::poses::CPose3D::getHomogeneousMatrixVal
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:229
CTraitsTest.h
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
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:388
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
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_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:400
mrpt::math::TPoint3D::x
double x
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:388
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:48
DEG2RAD
#define DEG2RAD
Definition: CColouredPointsMap.h:112
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:32
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
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
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_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
v
const GLdouble * v
Definition: glext.h:3678
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
mrpt::math::CQuaternion::x
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:89
mrpt::math::CQuaternion::normalizationJacobian
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
Definition: CQuaternion.h:283
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
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:429
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:453
Pose3DQuatTests::test_normalizeJacob
void test_normalizeJacob(double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:543
mrpt::math::CArrayDouble
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:87
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:414
y
GLenum GLint GLint y
Definition: glext.h:3538
x
GLenum GLint x
Definition: glext.h:3538
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
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:381
mrpt::math::CQuaternion::z
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:93
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:25
z1
INT32 z1
Definition: jidctint.cpp:130
Pose3DQuatTests::test_unaryInverse
void test_unaryInverse(double x1, double y1, double z1, double yaw1, double pitch1, double roll1)
Definition: CPose3DQuat_unittest.cpp:356
p
GLfloat GLfloat p
Definition: glext.h:6305
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
TEST_F
TEST_F(Pose3DQuatTests, FromYPRAndBack)
Definition: CPose3DQuat_unittest.cpp:583
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:723


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019