MRPT  1.9.9
CPose3DPDFGaussian_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>
13 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/random.h>
17 #include <Eigen/Dense>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 using namespace mrpt::math;
22 using namespace std;
23 
24 template class mrpt::CTraitsTest<CPose3DPDFGaussian>;
25 
26 class Pose3DPDFGaussTests : public ::testing::Test
27 {
28  protected:
29  void SetUp() override {}
30  void TearDown() override {}
32  double x, double y, double z, double yaw, double pitch, double roll,
33  double std_scale)
34  {
37  r, 0, std_scale);
39  cov.matProductOf_AAt(r); // random semi-definite positive matrix:
40  for (int i = 0; i < 6; i++) cov(i, i) += 1e-7;
41  CPose3DPDFGaussian p6pdf(CPose3D(x, y, z, yaw, pitch, roll), cov);
42  return p6pdf;
43  }
44 
46  double x, double y, double z, double yaw, double pitch, double roll,
47  double std_scale)
48  {
49  CPose3DPDFGaussian p6pdf =
50  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
51  // cout << "p6pdf: " << p6pdf << endl;
53  // cout << "p7pdf: " << p7pdf << endl;
54  CPose3DPDFGaussian p6pdf_recov = CPose3DPDFGaussian(p7pdf);
55  // cout << "p6pdf_recov: " << p6pdf_recov << endl;
56 
57  const double val_mean_error =
58  (p6pdf_recov.mean.asVectorVal() - p6pdf.mean.asVectorVal())
59  .sum_abs();
60  const double cov_mean_error = (p6pdf_recov.cov - p6pdf.cov).sum_abs();
61  // cout << "cov err: " << cov_mean_error << " " << "val_mean_error: " <<
62  // val_mean_error << endl;
63  EXPECT_TRUE(val_mean_error < 1e-8);
64  EXPECT_TRUE(cov_mean_error < 1e-8);
65  }
66 
67  static void func_compose(
68  const CVectorFixedDouble<12>& x, const double& dummy,
70  {
71  MRPT_UNUSED_PARAM(dummy);
72  const CPose3D p1(x[0], x[1], x[2], x[3], x[4], x[5]);
73  const CPose3D p2(
74  x[6 + 0], x[6 + 1], x[6 + 2], x[6 + 3], x[6 + 4], x[6 + 5]);
75  const CPose3D p = p1 + p2;
76  for (int i = 0; i < 6; i++) Y[i] = p[i];
77  }
78 
79  static void func_inv_compose(
80  const CVectorFixedDouble<2 * 6>& x, const double& dummy,
82  {
83  MRPT_UNUSED_PARAM(dummy);
84  const CPose3D p1(x[0], x[1], x[2], x[3], x[4], x[5]);
85  const CPose3D p2(
86  x[6 + 0], x[6 + 1], x[6 + 2], x[6 + 3], x[6 + 4], x[6 + 5]);
87  const CPose3D p = p1 - p2;
88  for (int i = 0; i < 6; i++) Y[i] = p[i];
89  }
90 
91  // Test "+" & "+=" operator
93  double x, double y, double z, double yaw, double pitch, double roll,
94  double std_scale, double x2, double y2, double z2, double yaw2,
95  double pitch2, double roll2, double std_scale2)
96  {
97  CPose3DPDFGaussian p6pdf1 =
98  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
99  CPose3DPDFGaussian p6pdf2 = generateRandomPose3DPDF(
100  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
101 
102  // With "+" operators:
103  CPose3DPDFGaussian p6_comp = p6pdf1 + p6pdf2;
104 
105  // Numeric approximation:
106  CVectorFixedDouble<6> y_mean;
108  {
109  CVectorFixedDouble<12> x_mean;
110  for (int i = 0; i < 6; i++) x_mean[i] = p6pdf1.mean[i];
111  for (int i = 0; i < 6; i++) x_mean[6 + i] = p6pdf2.mean[i];
112 
114  x_cov.insertMatrix(0, 0, p6pdf1.cov);
115  x_cov.insertMatrix(6, 6, p6pdf2.cov);
116 
117  double DUMMY = 0;
118  CVectorFixedDouble<12> x_incrs;
119  x_incrs.fill(1e-6);
121  x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
122  }
123  // Compare mean:
124  EXPECT_NEAR(0, (y_mean - p6_comp.mean.asVectorVal()).sum_abs(), 1e-2)
125  << "p1 mean: " << p6pdf1.mean << endl
126  << "p2 mean: " << p6pdf2.mean << endl;
127 
128  // Compare cov:
129  EXPECT_NEAR(0, (y_cov - p6_comp.cov).sum_abs(), 1e-2)
130  << "p1 mean: " << p6pdf1.mean << endl
131  << "p2 mean: " << p6pdf2.mean << endl;
132 
133  // Test +=
134  p6_comp = p6pdf1;
135  p6_comp += p6pdf2;
136 
137  // Compare mean:
138  EXPECT_NEAR(0, (y_mean - p6_comp.mean.asVectorVal()).sum_abs(), 1e-2)
139  << "p1 mean: " << p6pdf1.mean << endl
140  << "p2 mean: " << p6pdf2.mean << endl;
141 
142  // Compare cov:
143  EXPECT_NEAR(0, (y_cov - p6_comp.cov).sum_abs(), 1e-2)
144  << "p1 mean: " << p6pdf1.mean << endl
145  << "p2 mean: " << p6pdf2.mean << endl;
146  }
147 
149  double x, double y, double z, double yaw, double pitch, double roll,
150  double x2, double y2, double z2, double yaw2, double pitch2,
151  double roll2)
152  {
153  const CPose3D q1(x, y, z, yaw, pitch, roll);
154  const CPose3D q2(x2, y2, z2, yaw2, pitch2, roll2);
155 
156  // Theoretical Jacobians:
158  df_du(UNINITIALIZED_MATRIX);
160  q1, // x
161  q2, // u
162  df_dx, df_du);
163 
164  // Numerical approximation:
166  num_df_du(UNINITIALIZED_MATRIX);
167  {
169  for (int i = 0; i < 6; i++) x_mean[i] = q1[i];
170  for (int i = 0; i < 6; i++) x_mean[6 + i] = q2[i];
171 
172  double DUMMY = 0;
174  x_incrs.fill(1e-7);
175  CMatrixDouble numJacobs;
177  x_mean,
178  std::function<void(
179  const CVectorFixedDouble<12>& x, const double& dummy,
180  CVectorFixedDouble<6>& Y)>(&func_compose),
181  x_incrs, DUMMY, numJacobs);
182 
183  num_df_dx = numJacobs.block<6, 6>(0, 0);
184  num_df_du = numJacobs.block<6, 6>(0, 6);
185  }
186 
187  // Compare:
188  EXPECT_NEAR(0, (df_dx - num_df_dx).sum_abs(), 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).sum_abs(), 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 
209  // Test the "-" & "-=" operator
211  double x, double y, double z, double yaw, double pitch, double roll,
212  double std_scale, double x2, double y2, double z2, double yaw2,
213  double pitch2, double roll2, double std_scale2)
214  {
215  CPose3DPDFGaussian p6pdf1 =
216  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
217  CPose3DPDFGaussian p6pdf2 = generateRandomPose3DPDF(
218  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
219 
220  // With the "-" operator
221  CPose3DPDFGaussian p6_comp = p6pdf1 - p6pdf2;
222 
223  // Numeric approximation:
224  CVectorFixedDouble<6> y_mean;
226  {
228  for (int i = 0; i < 6; i++) x_mean[i] = p6pdf1.mean[i];
229  for (int i = 0; i < 6; i++) x_mean[6 + i] = p6pdf2.mean[i];
230 
232  x_cov.insertMatrix(0, 0, p6pdf1.cov);
233  x_cov.insertMatrix(6, 6, p6pdf2.cov);
234 
235  double DUMMY = 0;
237  x_incrs.fill(1e-6);
239  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
240  }
241  // Compare mean:
242  EXPECT_NEAR(0, (y_mean - p6_comp.mean.asVectorVal()).sum_abs(), 1e-2)
243  << "p1 mean: " << p6pdf1.mean << endl
244  << "p2 mean: " << p6pdf2.mean << endl;
245 
246  // Compare cov:
247  EXPECT_NEAR(0, (y_cov - p6_comp.cov).sum_abs(), 1e-2)
248  << "p1 mean: " << p6pdf1.mean << endl
249  << "p2 mean: " << p6pdf2.mean << endl;
250 
251  // With the "-=" operator
252  p6_comp = p6pdf1;
253  p6_comp -= p6pdf2;
254 
255  // Compare mean:
256  EXPECT_NEAR(0, (y_mean - p6_comp.mean.asVectorVal()).sum_abs(), 1e-2)
257  << "p1 mean: " << p6pdf1.mean << endl
258  << "p2 mean: " << p6pdf2.mean << endl;
259 
260  // Compare cov:
261  EXPECT_NEAR(0, (y_cov - p6_comp.cov).sum_abs(), 1e-2)
262  << "p1 mean: " << p6pdf1.mean << endl
263  << "p2 mean: " << p6pdf2.mean << endl;
264  }
265 
266  // Test the unary "-" operator
268  double x, double y, double z, double yaw, double pitch, double roll,
269  double std_scale)
270  {
271  CPose3DPDFGaussian p6pdf2 =
272  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
273  CPose3DPDFGaussian p6_zero(
274  CPose3D(0, 0, 0, 0, 0, 0), CMatrixDouble66()); // COV=All zeros
275 
276  // Unary "-":
277  const CPose3DPDFGaussian p6_inv = -p6pdf2;
278 
279  // Compare to the binary "-" operator:
280  const CPose3DPDFGaussian p6_comp = p6_zero - p6pdf2;
281 
282  // Compare mean:
283  EXPECT_NEAR(
284  0,
285  (p6_inv.mean.asVectorVal() - p6_comp.mean.asVectorVal())
286  .array()
287  .abs()
288  .sum(),
289  1e-2)
290  << "p mean: " << p6pdf2.mean << endl;
291 
292  // Compare cov:
293  EXPECT_NEAR(0, (p6_inv.cov - p6_comp.cov).sum_abs(), 1e-2)
294  << "p mean: " << p6pdf2.mean << endl;
295 
296  // Compare to the "inverse()" method:
297  CPose3DPDFGaussian p6_inv2;
298  p6pdf2.inverse(p6_inv2);
299 
300  // Compare mean:
301  EXPECT_NEAR(
302  0,
303  (p6_inv2.mean.asVectorVal() - p6_comp.mean.asVectorVal())
304  .array()
305  .abs()
306  .sum(),
307  1e-2)
308  << "p mean: " << p6pdf2.mean << endl
309  << "p6_inv2 mean: " << p6_inv2.mean << endl
310  << "p6_comp mean: " << p6_comp.mean << endl;
311 
312  // Compare cov:
313  EXPECT_NEAR(0, (p6_inv2.cov - p6_comp.cov).sum_abs(), 1e-2)
314  << "p mean: " << p6pdf2.mean << endl
315  << "p6_inv2 mean: " << p6_inv2.mean << endl
316  << "p6_comp mean: " << p6_comp.mean << endl;
317  }
318 
319  // Test all operators
321  double x, double y, double z, double yaw, double pitch, double roll,
322  double std_scale, double x2, double y2, double z2, double yaw2,
323  double pitch2, double roll2, double std_scale2)
324  {
325  // +, +=
326  testPoseComposition(
327  x, y, z, yaw, pitch, roll, std_scale, x2, y2, z2, yaw2, pitch2,
328  roll2, std_scale2);
329  // -, -=, unary "-"
330  testPoseInverseComposition(
331  x, y, z, yaw, pitch, roll, std_scale, x2, y2, z2, yaw2, pitch2,
332  roll2, std_scale2);
333  // unitary "-" & ".inverse()"
334  testPoseInverse(x, y, z, yaw, pitch, roll, std_scale);
335  }
336 
338  double x, double y, double z, double yaw, double pitch, double roll,
339  double std_scale, double x2, double y2, double z2, double yaw2,
340  double pitch2, double roll2)
341  {
342  CPose3DPDFGaussian p6pdf1 =
343  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
344 
345  const CPose3D new_base = CPose3D(x2, y2, z2, yaw2, pitch2, roll2);
346  const CPose3DPDFGaussian new_base_pdf(
347  new_base, CMatrixDouble66()); // COV = Zeros
348 
349  const CPose3DPDFGaussian p6_new_base_pdf = new_base_pdf + p6pdf1;
350  p6pdf1.changeCoordinatesReference(new_base);
351 
352  // Compare:
353  EXPECT_NEAR(
354  0, (p6_new_base_pdf.cov - p6pdf1.cov).array().abs().mean(), 1e-2)
355  << "p1 mean: " << p6pdf1.mean << endl
356  << "new_base: " << new_base << endl;
357  EXPECT_NEAR(
358  0,
359  (p6_new_base_pdf.mean.asVectorVal() - p6pdf1.mean.asVectorVal())
360  .array()
361  .abs()
362  .mean(),
363  1e-2)
364  << "p1 mean: " << p6pdf1.mean << endl
365  << "new_base: " << new_base << endl;
366  }
367 };
368 
369 TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
370 {
371  testToQuatPDFAndBack(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
372  testToQuatPDFAndBack(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.2);
373 
374  testToQuatPDFAndBack(6, -2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
375  testToQuatPDFAndBack(6, -2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.2);
376 
377  testToQuatPDFAndBack(6, -2, -3, DEG2RAD(10), DEG2RAD(40), DEG2RAD(5), 0.1);
378  testToQuatPDFAndBack(6, -2, -3, DEG2RAD(10), DEG2RAD(40), DEG2RAD(5), 0.2);
379 
380  testToQuatPDFAndBack(
381  6, -2, -3, DEG2RAD(-50), DEG2RAD(87), DEG2RAD(20), 0.1);
382  testToQuatPDFAndBack(
383  6, -2, -3, DEG2RAD(-50), DEG2RAD(87), DEG2RAD(20), 0.2);
384 
385  testToQuatPDFAndBack(
386  6, -2, -3, DEG2RAD(-50), DEG2RAD(-87), DEG2RAD(20), 0.1);
387  testToQuatPDFAndBack(
388  6, -2, -3, DEG2RAD(-50), DEG2RAD(-87), DEG2RAD(20), 0.2);
389 }
390 
391 TEST_F(Pose3DPDFGaussTests, CompositionJacobian)
392 {
393  testCompositionJacobian(
394  0, 0, 0, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0),
395  DEG2RAD(0), DEG2RAD(0));
396  testCompositionJacobian(
397  0, 0, 0, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0),
398  DEG2RAD(0), DEG2RAD(0));
399  testCompositionJacobian(
400  1, 2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
401  DEG2RAD(0), DEG2RAD(0));
402  testCompositionJacobian(
403  1, -2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
404  DEG2RAD(0), DEG2RAD(0));
405  testCompositionJacobian(
406  1, 2, -3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
407  DEG2RAD(0), DEG2RAD(0));
408  testCompositionJacobian(
409  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
410  DEG2RAD(-10), DEG2RAD(30));
411  testCompositionJacobian(
412  1, 2, 3, DEG2RAD(20), DEG2RAD(-80), DEG2RAD(70), -8, 45, 10,
413  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
414  testCompositionJacobian(
415  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(-70), -8, 45, 10,
416  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
417  testCompositionJacobian(
418  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10,
419  DEG2RAD(-50), DEG2RAD(-10), DEG2RAD(30));
420  testCompositionJacobian(
421  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
422  DEG2RAD(10), DEG2RAD(30));
423  testCompositionJacobian(
424  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
425  DEG2RAD(-10), DEG2RAD(-30));
426 }
427 
428 // Test the +, -, +=, -=, "-" operators
430 {
431  testAllPoseOperators(
432  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
433  DEG2RAD(0), DEG2RAD(0), 0.1);
434  testAllPoseOperators(
435  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
436  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
437 
438  testAllPoseOperators(
439  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
440  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
441  testAllPoseOperators(
442  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
443  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
444 
445  testAllPoseOperators(
446  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
447  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
448  testAllPoseOperators(
449  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
450  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
451  testAllPoseOperators(
452  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
453  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
454  testAllPoseOperators(
455  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
456  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
457  testAllPoseOperators(
458  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
459  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
460  testAllPoseOperators(
461  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
462  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
463 }
464 
465 TEST_F(Pose3DPDFGaussTests, ChangeCoordsRef)
466 {
467  testChangeCoordsRef(
468  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
469  DEG2RAD(0), DEG2RAD(0));
470  testChangeCoordsRef(
471  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
472  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
473 
474  testChangeCoordsRef(
475  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
476  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
477  testChangeCoordsRef(
478  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
479  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
480 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
CPose3D mean
The mean value.
GLdouble GLdouble z
Definition: glext.h:3879
void testAllPoseOperators(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)
double DEG2RAD(const double x)
Degrees to radians.
static void jacobiansPoseComposition(const CPose3D &x, const CPose3D &u, mrpt::math::CMatrixDouble66 &df_dx, mrpt::math::CMatrixDouble66 &df_du)
This static method computes the pose composition Jacobians.
Definition: CPose3DPDF.cpp:129
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
TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
STL namespace.
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)
void testPoseInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
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.
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
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
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
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)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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 CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
void testToQuatPDFAndBack(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
CMatrixFixed< double, 6, 6 > CMatrixDouble66
Definition: CMatrixFixed.h:354
static void func_compose(const CVectorFixedDouble< 12 > &x, const double &dummy, CVectorFixedDouble< 6 > &Y)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
GLenum GLint GLint y
Definition: glext.h:3542
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)
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
GLenum GLint x
Definition: glext.h:3542
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 void func_inv_compose(const CVectorFixedDouble< 2 *6 > &x, const double &dummy, CVectorFixedDouble< 6 > &Y)



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