MRPT  1.9.9
CPose3DQuatPDFGaussian_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
12 #include <mrpt/math/num_jacobian.h>
14 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/random.h>
18 #include <Eigen/Dense>
19 
20 using namespace mrpt;
21 using namespace mrpt::poses;
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  void SetUp() override {}
31  void TearDown() override {}
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.matProductOf_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 CVectorFixedDouble<2 * 7>& x, const double& dummy,
76  {
77  MRPT_UNUSED_PARAM(dummy);
78  const CPose3DQuat p1(
79  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
80  const CPose3DQuat p2(
81  x[7 + 0], x[7 + 1], x[7 + 2],
82  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
83  const CPose3DQuat p = p1 + p2;
84  for (int i = 0; i < 7; i++) Y[i] = p[i];
85  }
86 
87  static void func_inv_compose(
88  const CVectorFixedDouble<2 * 7>& x, const double& dummy,
90  {
91  MRPT_UNUSED_PARAM(dummy);
92  const CPose3DQuat p1(
93  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
94  const CPose3DQuat p2(
95  x[7 + 0], x[7 + 1], x[7 + 2],
96  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
97  const CPose3DQuat p = p1 - p2;
98  for (int i = 0; i < 7; i++) Y[i] = p[i];
99  }
100 
102  double x, double y, double z, double yaw, double pitch, double roll,
103  double std_scale, double x2, double y2, double z2, double yaw2,
104  double pitch2, double roll2, double std_scale2)
105  {
106  CPose3DQuatPDFGaussian p7pdf1 =
107  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
108  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
109  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
110 
111  CPose3DQuatPDFGaussian p7_comp = p7pdf1 + p7pdf2;
112 
113  // Numeric approximation:
114  CVectorFixedDouble<7> y_mean;
116  {
118  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
119  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
120 
122  x_cov.insertMatrix(0, 0, p7pdf1.cov);
123  x_cov.insertMatrix(7, 7, p7pdf2.cov);
124 
125  double DUMMY = 0;
127  x_incrs.fill(1e-6);
129  x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
130  }
131  // Compare:
132  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
133  << "p1 mean: " << p7pdf1.mean << endl
134  << "p2 mean: " << p7pdf2.mean << endl
135  << "Numeric approximation of covariance: " << endl
136  << y_cov << endl
137  << "Returned covariance: " << endl
138  << p7_comp.cov << endl;
139  }
140 
141  static void func_inverse(
142  const CVectorFixedDouble<7>& x, const double& dummy,
144  {
145  MRPT_UNUSED_PARAM(dummy);
146  const CPose3DQuat p1(
147  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
148  const CPose3DQuat p1_inv(-p1);
149  for (int i = 0; i < 7; i++) Y[i] = p1_inv[i];
150  }
151 
153  double x, double y, double z, double yaw, double pitch, double roll,
154  double x2, double y2, double z2, double yaw2, double pitch2,
155  double roll2)
156  {
157  const CPose3DQuat q1(CPose3D(x, y, z, yaw, pitch, roll));
158  const CPose3DQuat q2(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
159 
160  // Theoretical Jacobians:
162  df_du(UNINITIALIZED_MATRIX);
164  q1, // x
165  q2, // u
166  df_dx, df_du);
167 
168  // Numerical approximation:
170  num_df_du(UNINITIALIZED_MATRIX);
171  {
173  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
174  for (int i = 0; i < 7; i++) x_mean[7 + i] = q2[i];
175 
176  double DUMMY = 0;
178  x_incrs.fill(1e-7);
179  CMatrixDouble numJacobs;
181  x_mean,
182  std::function<void(
183  const CVectorFixedDouble<2 * 7>& x, const double& dummy,
184  CVectorFixedDouble<7>& Y)>(&func_compose),
185  x_incrs, DUMMY, numJacobs);
186 
187  num_df_dx = numJacobs.block<7, 7>(0, 0);
188  num_df_du = numJacobs.block<7, 7>(0, 7);
189  }
190 
191  // Compare:
192  EXPECT_NEAR(0, (df_dx - num_df_dx).array().abs().sum(), 3e-3)
193  << "q1: " << q1 << endl
194  << "q2: " << q2 << endl
195  << "Numeric approximation of df_dx: " << endl
196  << num_df_dx << endl
197  << "Implemented method: " << endl
198  << df_dx << endl
199  << "Error: " << endl
200  << df_dx - num_df_dx << endl;
201 
202  EXPECT_NEAR(0, (df_du - num_df_du).array().abs().sum(), 3e-3)
203  << "q1: " << q1 << endl
204  << "q2: " << q2 << endl
205  << "Numeric approximation of df_du: " << endl
206  << num_df_du << endl
207  << "Implemented method: " << endl
208  << df_du << endl
209  << "Error: " << endl
210  << df_du - num_df_du << endl;
211  }
212 
214  double x, double y, double z, double yaw, double pitch, double roll,
215  double std_scale)
216  {
217  CPose3DQuatPDFGaussian p7pdf1 =
218  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
219 
220  CPose3DQuatPDFGaussian p7_inv = -p7pdf1;
221 
222  // Numeric approximation:
223  CVectorFixedDouble<7> y_mean;
225  {
226  CVectorFixedDouble<7> x_mean;
227  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
228 
230  x_cov.insertMatrix(0, 0, p7pdf1.cov);
231 
232  double DUMMY = 0;
233  CVectorFixedDouble<7> x_incrs;
234  x_incrs.fill(1e-6);
236  x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
237  }
238 
239  // Compare:
240  EXPECT_NEAR(0, (y_cov - p7_inv.cov).array().abs().mean(), 1e-2)
241  << "p1 mean: " << p7pdf1.mean << endl
242  << "inv mean: " << p7_inv.mean << endl
243  << "Numeric approximation of covariance: " << endl
244  << y_cov << endl
245  << "Returned covariance: " << endl
246  << p7_inv.cov << endl
247  << "Error: " << endl
248  << y_cov - p7_inv.cov << endl;
249  }
250 
252  double x, double y, double z, double yaw, double pitch, double roll,
253  double std_scale, double x2, double y2, double z2, double yaw2,
254  double pitch2, double roll2, double std_scale2)
255  {
256  CPose3DQuatPDFGaussian p7pdf1 =
257  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
258  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
259  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
260 
261  CPose3DQuatPDFGaussian p7_comp = p7pdf1 - p7pdf2;
262 
263  // Numeric approximation:
264  CVectorFixedDouble<7> y_mean;
266  {
268  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
269  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
270 
272  x_cov.insertMatrix(0, 0, p7pdf1.cov);
273  x_cov.insertMatrix(7, 7, p7pdf2.cov);
274 
275  double DUMMY = 0;
277  x_incrs.fill(1e-6);
279  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
280  }
281  // Compare:
282  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
283  << "p1 mean: " << p7pdf1.mean << endl
284  << "p2 mean: " << p7pdf2.mean << endl
285  << "Numeric approximation of covariance: " << endl
286  << y_cov << endl
287  << "Returned covariance: " << endl
288  << p7_comp.cov << endl;
289  }
290 
292  double x, double y, double z, double yaw, double pitch, double roll,
293  double std_scale, double x2, double y2, double z2, double yaw2,
294  double pitch2, double roll2)
295  {
296  CPose3DQuatPDFGaussian p7pdf1 =
297  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
298 
299  const CPose3DQuat new_base =
300  CPose3DQuat(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
301  const CPose3DQuatPDFGaussian new_base_pdf(
302  new_base, CMatrixDouble77()); // COV = Zeros
303 
304  const CPose3DQuatPDFGaussian p7_new_base_pdf = new_base_pdf + p7pdf1;
305  p7pdf1.changeCoordinatesReference(new_base);
306 
307  // Compare:
308  EXPECT_NEAR(
309  0, (p7_new_base_pdf.cov - p7pdf1.cov).array().abs().mean(), 1e-2)
310  << "p1 mean: " << p7pdf1.mean << endl
311  << "new_base: " << new_base << endl;
312  EXPECT_NEAR(
313  0,
314  (p7_new_base_pdf.mean.asVectorVal() - p7pdf1.mean.asVectorVal())
315  .array()
316  .abs()
317  .mean(),
318  1e-2)
319  << "p1 mean: " << p7pdf1.mean << endl
320  << "new_base: " << new_base << endl;
321  }
322 };
323 
324 TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
325 {
326  test_toFromYPRGauss(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
327  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(88), DEG2RAD(0));
328  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(89.5), DEG2RAD(0));
329  // The formulas break at pitch=90, but this we cannot avoid...
330 }
331 
332 TEST_F(Pose3DQuatPDFGaussTests, CompositionJacobian)
333 {
334  testCompositionJacobian(
335  0, 0, 0, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0),
336  DEG2RAD(0), DEG2RAD(0));
337  testCompositionJacobian(
338  1, 2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
339  DEG2RAD(0), DEG2RAD(0));
340  testCompositionJacobian(
341  1, -2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
342  DEG2RAD(0), DEG2RAD(0));
343  testCompositionJacobian(
344  1, 2, -3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
345  DEG2RAD(0), DEG2RAD(0));
346  testCompositionJacobian(
347  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
348  DEG2RAD(-10), DEG2RAD(30));
349  testCompositionJacobian(
350  1, 2, 3, DEG2RAD(20), DEG2RAD(-80), DEG2RAD(70), -8, 45, 10,
351  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
352  testCompositionJacobian(
353  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(-70), -8, 45, 10,
354  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
355  testCompositionJacobian(
356  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10,
357  DEG2RAD(-50), DEG2RAD(-10), DEG2RAD(30));
358  testCompositionJacobian(
359  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
360  DEG2RAD(10), DEG2RAD(30));
361  testCompositionJacobian(
362  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
363  DEG2RAD(-10), DEG2RAD(-30));
364 }
365 
367 {
368  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
369  testInverse(0, 0, 0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
370  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
371  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
372 
373  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
374  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.2);
375 
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(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
381  testInverse(-1, 2, 3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
382  testInverse(1, 2, -3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
383  testInverse(-1, 2, -3, DEG2RAD(-30), DEG2RAD(0), 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(-30), DEG2RAD(0), 0.1);
389  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
390  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
391  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 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  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
397  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
398  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
399  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
400 }
401 
403 {
404  testPoseComposition(
405  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
406  DEG2RAD(0), DEG2RAD(0), 0.1);
407  testPoseComposition(
408  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
409  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
410 
411  testPoseComposition(
412  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
413  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
414  testPoseComposition(
415  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
416  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
417 
418  testPoseComposition(
419  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
420  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
421  testPoseComposition(
422  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
423  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
424  testPoseComposition(
425  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
426  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
427  testPoseComposition(
428  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
429  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
430  testPoseComposition(
431  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
432  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
433  testPoseComposition(
434  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
435  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
436 }
437 
438 TEST_F(Pose3DQuatPDFGaussTests, InverseComposition)
439 {
440  testPoseInverseComposition(
441  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
442  DEG2RAD(0), DEG2RAD(0), 0.1);
443  testPoseInverseComposition(
444  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
445  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
446 
447  testPoseInverseComposition(
448  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
449  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
450  testPoseInverseComposition(
451  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
452  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
453 
454  testPoseInverseComposition(
455  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
456  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
457  testPoseInverseComposition(
458  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
459  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
460  testPoseInverseComposition(
461  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
462  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
463  testPoseInverseComposition(
464  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
465  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
466  testPoseInverseComposition(
467  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
468  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
469  testPoseInverseComposition(
470  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
471  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
472 }
473 
475 {
476  testChangeCoordsRef(
477  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
478  DEG2RAD(0), DEG2RAD(0));
479  testChangeCoordsRef(
480  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
481  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
482 
483  testChangeCoordsRef(
484  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
485  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
486  testChangeCoordsRef(
487  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
488  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
489 }
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
GLdouble GLdouble z
Definition: glext.h:3879
static CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
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.
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
Definition: MatrixBase.h:210
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:505
STL namespace.
CMatrixFixed< double, 7, 7 > CMatrixDouble77
Definition: CMatrixFixed.h:355
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:261
This base provides a set of functions for maths stuff.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
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...
CMatrixDouble 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
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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
static void func_inv_compose(const CVectorFixedDouble< 2 *7 > &x, const double &dummy, CVectorFixedDouble< 7 > &Y)
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.
static void func_inverse(const CVectorFixedDouble< 7 > &x, const double &dummy, CVectorFixedDouble< 7 > &Y)
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
static void func_compose(const CVectorFixedDouble< 2 *7 > &x, const double &dummy, CVectorFixedDouble< 7 > &Y)
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:3542
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:3542
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.
GLfloat GLfloat p
Definition: glext.h:6398
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)
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:266
#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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019