MRPT  1.9.9
CPose3DQuatPDFGaussian_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 
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/random.h>
15 #include <mrpt/math/num_jacobian.h>
16 #include <CTraitsTest.h>
17 #include <gtest/gtest.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 
22 using namespace mrpt::math;
23 using namespace std;
24 
25 template class mrpt::CTraitsTest<CPose3DQuatPDFGaussian>;
26 
27 class Pose3DQuatPDFGaussTests : public ::testing::Test
28 {
29  protected:
30  virtual void SetUp() {}
31  virtual void TearDown() {}
33  double x, double y, double z, double yaw, double pitch, double roll,
34  double std_scale)
35  {
37  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale));
38  }
39 
41  double x, double y, double z, double yaw, double pitch, double roll,
42  double std_scale)
43  {
46  r, 0, std_scale);
48  cov.multiply_AAt(r); // random semi-definite positive matrix:
49  for (int i = 0; i < 6; i++) cov(i, i) += 1e-7;
50  CPose3DPDFGaussian p6pdf(CPose3D(x, y, z, yaw, pitch, roll), cov);
51  return p6pdf;
52  }
53 
54  void test_toFromYPRGauss(double yaw, double pitch, double roll)
55  {
56  // Random pose:
57  CPose3DPDFGaussian p1ypr =
58  generateRandomPose3DPDF(1.0, 2.0, 3.0, yaw, pitch, roll, 0.1);
60 
61  // Convert back to a 6x6 representation:
62  CPose3DPDFGaussian p2ypr = CPose3DPDFGaussian(p1quat);
63 
64  EXPECT_NEAR(0, (p2ypr.cov - p1ypr.cov).array().abs().mean(), 1e-2)
65  << "p1ypr: " << endl
66  << p1ypr << endl
67  << "p1quat : " << endl
68  << p1quat << endl
69  << "p2ypr : " << endl
70  << p2ypr << endl;
71  }
72 
73  static void func_compose(
74  const CArrayDouble<2 * 7>& x, const double& dummy, CArrayDouble<7>& Y)
75  {
76  MRPT_UNUSED_PARAM(dummy);
77  const CPose3DQuat p1(
78  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
79  const CPose3DQuat p2(
80  x[7 + 0], x[7 + 1], x[7 + 2],
81  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
82  const CPose3DQuat p = p1 + p2;
83  for (int i = 0; i < 7; i++) Y[i] = p[i];
84  }
85 
86  static void func_inv_compose(
87  const CArrayDouble<2 * 7>& x, const double& dummy, CArrayDouble<7>& Y)
88  {
89  MRPT_UNUSED_PARAM(dummy);
90  const CPose3DQuat p1(
91  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
92  const CPose3DQuat p2(
93  x[7 + 0], x[7 + 1], x[7 + 2],
94  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
95  const CPose3DQuat p = p1 - p2;
96  for (int i = 0; i < 7; i++) Y[i] = p[i];
97  }
98 
100  double x, double y, double z, double yaw, double pitch, double roll,
101  double std_scale, double x2, double y2, double z2, double yaw2,
102  double pitch2, double roll2, double std_scale2)
103  {
104  CPose3DQuatPDFGaussian p7pdf1 =
105  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
106  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
107  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
108 
109  CPose3DQuatPDFGaussian p7_comp = p7pdf1 + p7pdf2;
110 
111  // Numeric approximation:
112  CArrayDouble<7> y_mean;
114  {
115  CArrayDouble<2 * 7> x_mean;
116  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
117  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
118 
120  x_cov.insertMatrix(0, 0, p7pdf1.cov);
121  x_cov.insertMatrix(7, 7, p7pdf2.cov);
122 
123  double DUMMY = 0;
124  CArrayDouble<2 * 7> x_incrs;
125  x_incrs.assign(1e-6);
127  x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
128  }
129  // Compare:
130  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
131  << "p1 mean: " << p7pdf1.mean << endl
132  << "p2 mean: " << p7pdf2.mean << endl
133  << "Numeric approximation of covariance: " << endl
134  << y_cov << endl
135  << "Returned covariance: " << endl
136  << p7_comp.cov << endl;
137  }
138 
139  static void func_inverse(
140  const CArrayDouble<7>& x, const double& dummy, CArrayDouble<7>& Y)
141  {
142  MRPT_UNUSED_PARAM(dummy);
143  const CPose3DQuat p1(
144  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
145  const CPose3DQuat p1_inv(-p1);
146  for (int i = 0; i < 7; i++) Y[i] = p1_inv[i];
147  }
148 
150  double x, double y, double z, double yaw, double pitch, double roll,
151  double x2, double y2, double z2, double yaw2, double pitch2,
152  double roll2)
153  {
154  const CPose3DQuat q1(CPose3D(x, y, z, yaw, pitch, roll));
155  const CPose3DQuat q2(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
156 
157  // Theoretical Jacobians:
159  df_du(UNINITIALIZED_MATRIX);
161  q1, // x
162  q2, // u
163  df_dx, df_du);
164 
165  // Numerical approximation:
167  num_df_du(UNINITIALIZED_MATRIX);
168  {
169  CArrayDouble<2 * 7> x_mean;
170  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
171  for (int i = 0; i < 7; i++) x_mean[7 + i] = q2[i];
172 
173  double DUMMY = 0;
174  CArrayDouble<2 * 7> x_incrs;
175  x_incrs.assign(1e-7);
176  CMatrixDouble numJacobs;
178  x_mean, std::function<void(
179  const CArrayDouble<2 * 7>& x, const double& dummy,
180  CArrayDouble<7>& Y)>(&func_compose),
181  x_incrs, DUMMY, numJacobs);
182 
183  numJacobs.extractMatrix(0, 0, num_df_dx);
184  numJacobs.extractMatrix(0, 7, num_df_du);
185  }
186 
187  // Compare:
188  EXPECT_NEAR(0, (df_dx - num_df_dx).array().abs().sum(), 3e-3)
189  << "q1: " << q1 << endl
190  << "q2: " << q2 << endl
191  << "Numeric approximation of df_dx: " << endl
192  << num_df_dx << endl
193  << "Implemented method: " << endl
194  << df_dx << endl
195  << "Error: " << endl
196  << df_dx - num_df_dx << endl;
197 
198  EXPECT_NEAR(0, (df_du - num_df_du).array().abs().sum(), 3e-3)
199  << "q1: " << q1 << endl
200  << "q2: " << q2 << endl
201  << "Numeric approximation of df_du: " << endl
202  << num_df_du << endl
203  << "Implemented method: " << endl
204  << df_du << endl
205  << "Error: " << endl
206  << df_du - num_df_du << endl;
207  }
208 
210  double x, double y, double z, double yaw, double pitch, double roll,
211  double std_scale)
212  {
213  CPose3DQuatPDFGaussian p7pdf1 =
214  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
215 
216  CPose3DQuatPDFGaussian p7_inv = -p7pdf1;
217 
218  // Numeric approximation:
219  CArrayDouble<7> y_mean;
221  {
222  CArrayDouble<7> x_mean;
223  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
224 
226  x_cov.insertMatrix(0, 0, p7pdf1.cov);
227 
228  double DUMMY = 0;
229  CArrayDouble<7> x_incrs;
230  x_incrs.assign(1e-6);
232  x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
233  }
234 
235  // Compare:
236  EXPECT_NEAR(0, (y_cov - p7_inv.cov).array().abs().mean(), 1e-2)
237  << "p1 mean: " << p7pdf1.mean << endl
238  << "inv mean: " << p7_inv.mean << endl
239  << "Numeric approximation of covariance: " << endl
240  << y_cov << endl
241  << "Returned covariance: " << endl
242  << p7_inv.cov << endl
243  << "Error: " << endl
244  << y_cov - p7_inv.cov << endl;
245  }
246 
248  double x, double y, double z, double yaw, double pitch, double roll,
249  double std_scale, double x2, double y2, double z2, double yaw2,
250  double pitch2, double roll2, double std_scale2)
251  {
252  CPose3DQuatPDFGaussian p7pdf1 =
253  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
254  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
255  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
256 
257  CPose3DQuatPDFGaussian p7_comp = p7pdf1 - p7pdf2;
258 
259  // Numeric approximation:
260  CArrayDouble<7> y_mean;
262  {
263  CArrayDouble<2 * 7> x_mean;
264  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
265  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
266 
268  x_cov.insertMatrix(0, 0, p7pdf1.cov);
269  x_cov.insertMatrix(7, 7, p7pdf2.cov);
270 
271  double DUMMY = 0;
272  CArrayDouble<2 * 7> x_incrs;
273  x_incrs.assign(1e-6);
275  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
276  }
277  // Compare:
278  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
279  << "p1 mean: " << p7pdf1.mean << endl
280  << "p2 mean: " << p7pdf2.mean << endl
281  << "Numeric approximation of covariance: " << endl
282  << y_cov << endl
283  << "Returned covariance: " << endl
284  << p7_comp.cov << endl;
285  }
286 
288  double x, double y, double z, double yaw, double pitch, double roll,
289  double std_scale, double x2, double y2, double z2, double yaw2,
290  double pitch2, double roll2)
291  {
292  CPose3DQuatPDFGaussian p7pdf1 =
293  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
294 
295  const CPose3DQuat new_base =
296  CPose3DQuat(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
297  const CPose3DQuatPDFGaussian new_base_pdf(
298  new_base, CMatrixDouble77()); // COV = Zeros
299 
300  const CPose3DQuatPDFGaussian p7_new_base_pdf = new_base_pdf + p7pdf1;
301  p7pdf1.changeCoordinatesReference(new_base);
302 
303  // Compare:
304  EXPECT_NEAR(
305  0, (p7_new_base_pdf.cov - p7pdf1.cov).array().abs().mean(), 1e-2)
306  << "p1 mean: " << p7pdf1.mean << endl
307  << "new_base: " << new_base << endl;
308  EXPECT_NEAR(
309  0, (p7_new_base_pdf.mean.getAsVectorVal() -
310  p7pdf1.mean.getAsVectorVal())
311  .array()
312  .abs()
313  .mean(),
314  1e-2)
315  << "p1 mean: " << p7pdf1.mean << endl
316  << "new_base: " << new_base << endl;
317  }
318 };
319 
320 TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
321 {
322  test_toFromYPRGauss(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
323  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(88), DEG2RAD(0));
324  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(89.5), DEG2RAD(0));
325  // The formulas break at pitch=90, but this we cannot avoid...
326 }
327 
328 TEST_F(Pose3DQuatPDFGaussTests, CompositionJacobian)
329 {
330  testCompositionJacobian(
331  0, 0, 0, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0),
332  DEG2RAD(0), DEG2RAD(0));
333  testCompositionJacobian(
334  1, 2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
335  DEG2RAD(0), DEG2RAD(0));
336  testCompositionJacobian(
337  1, -2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
338  DEG2RAD(0), DEG2RAD(0));
339  testCompositionJacobian(
340  1, 2, -3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
341  DEG2RAD(0), DEG2RAD(0));
342  testCompositionJacobian(
343  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
344  DEG2RAD(-10), DEG2RAD(30));
345  testCompositionJacobian(
346  1, 2, 3, DEG2RAD(20), DEG2RAD(-80), DEG2RAD(70), -8, 45, 10,
347  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
348  testCompositionJacobian(
349  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(-70), -8, 45, 10,
350  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
351  testCompositionJacobian(
352  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10,
353  DEG2RAD(-50), DEG2RAD(-10), DEG2RAD(30));
354  testCompositionJacobian(
355  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
356  DEG2RAD(10), DEG2RAD(30));
357  testCompositionJacobian(
358  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
359  DEG2RAD(-10), DEG2RAD(-30));
360 }
361 
363 {
364  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
365  testInverse(0, 0, 0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
366  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
367  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
368 
369  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
370  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.2);
371 
372  testInverse(1, 2, 3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
373  testInverse(-1, 2, 3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
374  testInverse(1, 2, -3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
375  testInverse(-1, 2, -3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
376  testInverse(1, 2, 3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
377  testInverse(-1, 2, 3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
378  testInverse(1, 2, -3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
379  testInverse(-1, 2, -3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
380  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
381  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
382  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
383  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
384  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
385  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
386  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
387  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
388  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
389  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
390  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
391  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
392  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
393  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
394  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
395  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
396 }
397 
399 {
400  testPoseComposition(
401  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
402  DEG2RAD(0), DEG2RAD(0), 0.1);
403  testPoseComposition(
404  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
405  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
406 
407  testPoseComposition(
408  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
409  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
410  testPoseComposition(
411  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
412  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
413 
414  testPoseComposition(
415  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
416  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
417  testPoseComposition(
418  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
419  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
420  testPoseComposition(
421  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
422  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
423  testPoseComposition(
424  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
425  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
426  testPoseComposition(
427  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
428  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
429  testPoseComposition(
430  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
431  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
432 }
433 
434 TEST_F(Pose3DQuatPDFGaussTests, InverseComposition)
435 {
436  testPoseInverseComposition(
437  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
438  DEG2RAD(0), DEG2RAD(0), 0.1);
439  testPoseInverseComposition(
440  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
441  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
442 
443  testPoseInverseComposition(
444  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
445  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
446  testPoseInverseComposition(
447  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
448  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
449 
450  testPoseInverseComposition(
451  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
452  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
453  testPoseInverseComposition(
454  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
455  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
456  testPoseInverseComposition(
457  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
458  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
459  testPoseInverseComposition(
460  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
461  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
462  testPoseInverseComposition(
463  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
464  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
465  testPoseInverseComposition(
466  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
467  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
468 }
469 
471 {
472  testChangeCoordsRef(
473  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
474  DEG2RAD(0), DEG2RAD(0));
475  testChangeCoordsRef(
476  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
477  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
478 
479  testChangeCoordsRef(
480  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
481  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
482  testChangeCoordsRef(
483  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
484  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
485 }
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
GLdouble GLdouble z
Definition: glext.h:3872
static CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
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
void test_toFromYPRGauss(double yaw, double pitch, double roll)
TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
double DEG2RAD(const double x)
Degrees to radians.
void drawGaussian1DMatrix(MAT &matrix, const double mean=0, const double std=1)
Fills the given matrix with independent, 1D-normally distributed samples.
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:502
STL namespace.
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
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:29
static void func_compose(const CArrayDouble< 2 *7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
CMatrixFixedNumeric< double, 7, 7 > CMatrixDouble77
Definition: eigen_frwds.h:60
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:46
void transform_gaussian_linear(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const VECTORLIKE1 &x_increments)
First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for a...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void testInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
void testChangeCoordsRef(double x, double y, double z, double yaw, double pitch, double roll, double std_scale, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
GLenum GLint GLint y
Definition: glext.h:3538
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:148
static void func_inverse(const CArrayDouble< 7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
void testCompositionJacobian(double x, double y, double z, double yaw, double pitch, double roll, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
GLenum GLint x
Definition: glext.h:3538
static void jacobiansPoseComposition(const CPose3DQuat &x, const CPose3DQuat &u, mrpt::math::CMatrixDouble77 &df_dx, mrpt::math::CMatrixDouble77 &df_du, CPose3DQuat *out_x_oplus_u=nullptr)
This static method computes the two Jacobians of a pose composition operation $f(x,u)= x u$.
void testPoseInverseComposition(double x, double y, double z, double yaw, double pitch, double roll, double std_scale, double x2, double y2, double z2, double yaw2, double pitch2, double roll2, double std_scale2)
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
static void func_inv_compose(const CArrayDouble< 2 *7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
GLfloat GLfloat p
Definition: glext.h:6305
void testPoseComposition(double x, double y, double z, double yaw, double pitch, double roll, double std_scale, double x2, double y2, double z2, double yaw2, double pitch2, double roll2, double std_scale2)
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
static CPose3DQuatPDFGaussian generateRandomPoseQuat3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020