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>
12 #include <mrpt/math/transform_gaussian.h>
13 #include <mrpt/poses/CPose3D.h>
14 #include <mrpt/poses/CPose3DPDFGaussian.h>
15 #include <mrpt/poses/CPose3DQuatPDFGaussian.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 {}
31  static CPose3DPDFGaussian generateRandomPose3DPDF(
32  double x, double y, double z, double yaw, double pitch, double roll,
33  double std_scale)
34  {
35  CMatrixDouble61 r;
36  mrpt::random::getRandomGenerator().drawGaussian1DMatrix(
37  r, 0, std_scale);
38  CMatrixDouble66 cov;
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 
45  void testToQuatPDFAndBack(
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;
52  CPose3DQuatPDFGaussian p7pdf = CPose3DQuatPDFGaussian(p6pdf);
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,
69  CVectorFixedDouble<6>& Y)
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,
81  CVectorFixedDouble<6>& Y)
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
92  void testPoseComposition(
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;
107  CMatrixFixed<double, 6, 6> y_cov;
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 
113  CMatrixFixed<double, 12, 12> x_cov;
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);
120  transform_gaussian_linear(
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 
148  void testCompositionJacobian(
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:
157  CMatrixDouble66 df_dx(UNINITIALIZED_MATRIX),
158  df_du(UNINITIALIZED_MATRIX);
159  CPose3DPDF::jacobiansPoseComposition(
160  q1, // x
161  q2, // u
162  df_dx, df_du);
163 
164  // Numerical approximation:
165  CMatrixDouble66 num_df_dx(UNINITIALIZED_MATRIX),
166  num_df_du(UNINITIALIZED_MATRIX);
167  {
168  CVectorFixedDouble<2 * 6> x_mean;
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;
173  CVectorFixedDouble<2 * 6> x_incrs;
174  x_incrs.fill(1e-7);
175  CMatrixDouble numJacobs;
176  mrpt::math::estimateJacobian(
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
210  void testPoseInverseComposition(
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;
225  CMatrixFixed<double, 6, 6> y_cov;
226  {
227  CVectorFixedDouble<2 * 6> x_mean;
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 
231  CMatrixFixed<double, 12, 12> x_cov;
232  x_cov.insertMatrix(0, 0, p6pdf1.cov);
233  x_cov.insertMatrix(6, 6, p6pdf2.cov);
234 
235  double DUMMY = 0;
236  CVectorFixedDouble<2 * 6> x_incrs;
237  x_incrs.fill(1e-6);
238  transform_gaussian_linear(
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
267  void testPoseInverse(
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
320  void testAllPoseOperators(
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 
337  void testChangeCoordsRef(
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
429 TEST_F(Pose3DPDFGaussTests, AllOperators)
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 }
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:78
z
GLdouble GLdouble z
Definition: glext.h:3879
Pose3DPDFGaussTests::testAllPoseOperators
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)
Definition: CPose3DPDFGaussian_unittest.cpp:320
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::poses::CPose3DPDF::jacobiansPoseComposition
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
mrpt::random::CRandomGenerator::drawGaussian1DMatrix
void drawGaussian1DMatrix(MAT &matrix, const double mean=0, const double std=1)
Fills the given matrix with independent, 1D-normally distributed samples.
Definition: RandomGenerators.h:204
mrpt::math::MatrixBase::insertMatrix
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
TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
Definition: CPose3DPDFGaussian_unittest.cpp:369
std
STL namespace.
CPose3D.h
Pose3DPDFGaussTests::testCompositionJacobian
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)
Definition: CPose3DPDFGaussian_unittest.cpp:148
Pose3DPDFGaussTests::testPoseInverse
void testPoseInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DPDFGaussian_unittest.cpp:267
mrpt::poses::CPose3DQuatPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
Definition: CPose3DQuatPDFGaussian.h:43
mrpt::math::MatrixBase::matProductOf_AAt
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:261
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::poses::CPose3DPDFGaussian::inverse
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Definition: CPose3DPDFGaussian.cpp:412
mrpt::math::MatrixVectorBase::block
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
Definition: MatrixVectorBase.h:138
mrpt::math::transform_gaussian_linear
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...
Definition: transform_gaussian.h:152
mrpt::math::cov
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
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::math::estimateJacobian
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:31
CPose3DPDFGaussian.h
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:82
Pose3DPDFGaussTests::testChangeCoordsRef
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)
Definition: CPose3DPDFGaussian_unittest.cpp:337
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:27
transform_gaussian.h
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
Pose3DPDFGaussTests::generateRandomPose3DPDF
static CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DPDFGaussian_unittest.cpp:31
Pose3DPDFGaussTests::testToQuatPDFAndBack
void testToQuatPDFAndBack(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DPDFGaussian_unittest.cpp:45
mrpt::math::CMatrixDouble66
CMatrixFixed< double, 6, 6 > CMatrixDouble66
Definition: CMatrixFixed.h:354
Pose3DPDFGaussTests::func_compose
static void func_compose(const CVectorFixedDouble< 12 > &x, const double &dummy, CVectorFixedDouble< 6 > &Y)
Definition: CPose3DPDFGaussian_unittest.cpp:67
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:39
y
GLenum GLint GLint y
Definition: glext.h:3542
Pose3DPDFGaussTests::testPoseInverseComposition
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)
Definition: CPose3DPDFGaussian_unittest.cpp:210
mrpt::poses::CPose3DPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CPose3DPDFGaussian.cpp:300
x
GLenum GLint x
Definition: glext.h:3542
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
p
GLfloat GLfloat p
Definition: glext.h:6398
Pose3DPDFGaussTests::testPoseComposition
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)
Definition: CPose3DPDFGaussian_unittest.cpp:92
mrpt::poses::CPoseOrPoint::asVectorVal
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
CPose3DQuatPDFGaussian.h
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
Pose3DPDFGaussTests::func_inv_compose
static void func_inv_compose(const CVectorFixedDouble< 2 *6 > &x, const double &dummy, CVectorFixedDouble< 6 > &Y)
Definition: CPose3DPDFGaussian_unittest.cpp:79


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