MRPT  1.9.9
CPose3DPDFGaussian_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 
10 #include <mrpt/poses/CPose3D.h>
11 #include <mrpt/poses/CPose3DPDFGaussian.h>
12 #include <mrpt/poses/CPose3DQuatPDFGaussian.h>
13 #include <mrpt/random.h>
14 #include <mrpt/math/transform_gaussian.h>
15 #include <CTraitsTest.h>
16 #include <gtest/gtest.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::poses;
20 
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  virtual void SetUp() {}
30  virtual void TearDown() {}
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.multiply_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.getAsVectorVal() - p6pdf.mean.getAsVectorVal())
59  .array()
60  .abs()
61  .mean();
62  const double cov_mean_error =
63  (p6pdf_recov.cov - p6pdf.cov).array().abs().mean();
64  // cout << "cov err: " << cov_mean_error << " " << "val_mean_error: " <<
65  // val_mean_error << endl;
66  EXPECT_TRUE(val_mean_error < 1e-8);
67  EXPECT_TRUE(cov_mean_error < 1e-8);
68  }
69 
70  static void func_compose(
71  const CArrayDouble<12>& x, const double& dummy, CArrayDouble<6>& Y)
72  {
73  MRPT_UNUSED_PARAM(dummy);
74  const CPose3D p1(x[0], x[1], x[2], x[3], x[4], x[5]);
75  const CPose3D p2(
76  x[6 + 0], x[6 + 1], x[6 + 2], x[6 + 3], x[6 + 4], x[6 + 5]);
77  const CPose3D p = p1 + p2;
78  for (int i = 0; i < 6; i++) Y[i] = p[i];
79  }
80 
81  static void func_inv_compose(
82  const CArrayDouble<2 * 6>& x, const double& dummy, CArrayDouble<6>& Y)
83  {
84  MRPT_UNUSED_PARAM(dummy);
85  const CPose3D p1(x[0], x[1], x[2], x[3], x[4], x[5]);
86  const CPose3D p2(
87  x[6 + 0], x[6 + 1], x[6 + 2], x[6 + 3], x[6 + 4], x[6 + 5]);
88  const CPose3D p = p1 - p2;
89  for (int i = 0; i < 6; i++) Y[i] = p[i];
90  }
91 
92  // Test "+" & "+=" operator
93  void testPoseComposition(
94  double x, double y, double z, double yaw, double pitch, double roll,
95  double std_scale, double x2, double y2, double z2, double yaw2,
96  double pitch2, double roll2, double std_scale2)
97  {
98  CPose3DPDFGaussian p6pdf1 =
99  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
100  CPose3DPDFGaussian p6pdf2 = generateRandomPose3DPDF(
101  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
102 
103  // With "+" operators:
104  CPose3DPDFGaussian p6_comp = p6pdf1 + p6pdf2;
105 
106  // Numeric approximation:
107  CArrayDouble<6> y_mean;
108  CMatrixFixedNumeric<double, 6, 6> y_cov;
109  {
110  CArrayDouble<12> x_mean;
111  for (int i = 0; i < 6; i++) x_mean[i] = p6pdf1.mean[i];
112  for (int i = 0; i < 6; i++) x_mean[6 + i] = p6pdf2.mean[i];
113 
114  CMatrixFixedNumeric<double, 12, 12> x_cov;
115  x_cov.insertMatrix(0, 0, p6pdf1.cov);
116  x_cov.insertMatrix(6, 6, p6pdf2.cov);
117 
118  double DUMMY = 0;
119  CArrayDouble<12> x_incrs;
120  x_incrs.assign(1e-6);
121  transform_gaussian_linear(
122  x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
123  }
124  // Compare mean:
125  EXPECT_NEAR(
126  0, (y_mean - p6_comp.mean.getAsVectorVal()).array().abs().sum(),
127  1e-2)
128  << "p1 mean: " << p6pdf1.mean << endl
129  << "p2 mean: " << p6pdf2.mean << endl;
130 
131  // Compare cov:
132  EXPECT_NEAR(0, (y_cov - p6_comp.cov).array().abs().sum(), 1e-2)
133  << "p1 mean: " << p6pdf1.mean << endl
134  << "p2 mean: " << p6pdf2.mean << endl;
135 
136  // Test +=
137  p6_comp = p6pdf1;
138  p6_comp += p6pdf2;
139 
140  // Compare mean:
141  EXPECT_NEAR(
142  0, (y_mean - p6_comp.mean.getAsVectorVal()).array().abs().sum(),
143  1e-2)
144  << "p1 mean: " << p6pdf1.mean << endl
145  << "p2 mean: " << p6pdf2.mean << endl;
146 
147  // Compare cov:
148  EXPECT_NEAR(0, (y_cov - p6_comp.cov).array().abs().sum(), 1e-2)
149  << "p1 mean: " << p6pdf1.mean << endl
150  << "p2 mean: " << p6pdf2.mean << endl;
151  }
152 
153  void testCompositionJacobian(
154  double x, double y, double z, double yaw, double pitch, double roll,
155  double x2, double y2, double z2, double yaw2, double pitch2,
156  double roll2)
157  {
158  const CPose3D q1(x, y, z, yaw, pitch, roll);
159  const CPose3D q2(x2, y2, z2, yaw2, pitch2, roll2);
160 
161  // Theoretical Jacobians:
162  CMatrixDouble66 df_dx(UNINITIALIZED_MATRIX),
163  df_du(UNINITIALIZED_MATRIX);
164  CPose3DPDF::jacobiansPoseComposition(
165  q1, // x
166  q2, // u
167  df_dx, df_du);
168 
169  // Numerical approximation:
170  CMatrixDouble66 num_df_dx(UNINITIALIZED_MATRIX),
171  num_df_du(UNINITIALIZED_MATRIX);
172  {
173  CArrayDouble<2 * 6> x_mean;
174  for (int i = 0; i < 6; i++) x_mean[i] = q1[i];
175  for (int i = 0; i < 6; i++) x_mean[6 + i] = q2[i];
176 
177  double DUMMY = 0;
178  CArrayDouble<2 * 6> x_incrs;
179  x_incrs.assign(1e-7);
180  CMatrixDouble numJacobs;
181  mrpt::math::estimateJacobian(
182  x_mean, std::function<void(
183  const CArrayDouble<12>& x, const double& dummy,
184  CArrayDouble<6>& Y)>(&func_compose),
185  x_incrs, DUMMY, numJacobs);
186 
187  numJacobs.extractMatrix(0, 0, num_df_dx);
188  numJacobs.extractMatrix(0, 6, num_df_du);
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 
213  // Test the "-" & "-=" operator
214  void testPoseInverseComposition(
215  double x, double y, double z, double yaw, double pitch, double roll,
216  double std_scale, double x2, double y2, double z2, double yaw2,
217  double pitch2, double roll2, double std_scale2)
218  {
219  CPose3DPDFGaussian p6pdf1 =
220  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
221  CPose3DPDFGaussian p6pdf2 = generateRandomPose3DPDF(
222  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
223 
224  // With the "-" operator
225  CPose3DPDFGaussian p6_comp = p6pdf1 - p6pdf2;
226 
227  // Numeric approximation:
228  CArrayDouble<6> y_mean;
229  CMatrixFixedNumeric<double, 6, 6> y_cov;
230  {
231  CArrayDouble<2 * 6> x_mean;
232  for (int i = 0; i < 6; i++) x_mean[i] = p6pdf1.mean[i];
233  for (int i = 0; i < 6; i++) x_mean[6 + i] = p6pdf2.mean[i];
234 
235  CMatrixFixedNumeric<double, 12, 12> x_cov;
236  x_cov.insertMatrix(0, 0, p6pdf1.cov);
237  x_cov.insertMatrix(6, 6, p6pdf2.cov);
238 
239  double DUMMY = 0;
240  CArrayDouble<2 * 6> x_incrs;
241  x_incrs.assign(1e-6);
242  transform_gaussian_linear(
243  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
244  }
245  // Compare mean:
246  EXPECT_NEAR(
247  0, (y_mean - p6_comp.mean.getAsVectorVal()).array().abs().sum(),
248  1e-2)
249  << "p1 mean: " << p6pdf1.mean << endl
250  << "p2 mean: " << p6pdf2.mean << endl;
251 
252  // Compare cov:
253  EXPECT_NEAR(0, (y_cov - p6_comp.cov).array().abs().sum(), 1e-2)
254  << "p1 mean: " << p6pdf1.mean << endl
255  << "p2 mean: " << p6pdf2.mean << endl;
256 
257  // With the "-=" operator
258  p6_comp = p6pdf1;
259  p6_comp -= p6pdf2;
260 
261  // Compare mean:
262  EXPECT_NEAR(
263  0, (y_mean - p6_comp.mean.getAsVectorVal()).array().abs().sum(),
264  1e-2)
265  << "p1 mean: " << p6pdf1.mean << endl
266  << "p2 mean: " << p6pdf2.mean << endl;
267 
268  // Compare cov:
269  EXPECT_NEAR(0, (y_cov - p6_comp.cov).array().abs().sum(), 1e-2)
270  << "p1 mean: " << p6pdf1.mean << endl
271  << "p2 mean: " << p6pdf2.mean << endl;
272  }
273 
274  // Test the unary "-" operator
275  void testPoseInverse(
276  double x, double y, double z, double yaw, double pitch, double roll,
277  double std_scale)
278  {
279  CPose3DPDFGaussian p6pdf2 =
280  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
281  CPose3DPDFGaussian p6_zero(
282  CPose3D(0, 0, 0, 0, 0, 0), CMatrixDouble66()); // COV=All zeros
283 
284  // Unary "-":
285  const CPose3DPDFGaussian p6_inv = -p6pdf2;
286 
287  // Compare to the binary "-" operator:
288  const CPose3DPDFGaussian p6_comp = p6_zero - p6pdf2;
289 
290  // Compare mean:
291  EXPECT_NEAR(
292  0, (p6_inv.mean.getAsVectorVal() - p6_comp.mean.getAsVectorVal())
293  .array()
294  .abs()
295  .sum(),
296  1e-2)
297  << "p mean: " << p6pdf2.mean << endl;
298 
299  // Compare cov:
300  EXPECT_NEAR(0, (p6_inv.cov - p6_comp.cov).array().abs().sum(), 1e-2)
301  << "p mean: " << p6pdf2.mean << endl;
302 
303  // Compare to the "inverse()" method:
304  CPose3DPDFGaussian p6_inv2;
305  p6pdf2.inverse(p6_inv2);
306 
307  // Compare mean:
308  EXPECT_NEAR(
309  0, (p6_inv2.mean.getAsVectorVal() - p6_comp.mean.getAsVectorVal())
310  .array()
311  .abs()
312  .sum(),
313  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  // Compare cov:
319  EXPECT_NEAR(0, (p6_inv2.cov - p6_comp.cov).array().abs().sum(), 1e-2)
320  << "p mean: " << p6pdf2.mean << endl
321  << "p6_inv2 mean: " << p6_inv2.mean << endl
322  << "p6_comp mean: " << p6_comp.mean << endl;
323  }
324 
325  // Test all operators
326  void testAllPoseOperators(
327  double x, double y, double z, double yaw, double pitch, double roll,
328  double std_scale, double x2, double y2, double z2, double yaw2,
329  double pitch2, double roll2, double std_scale2)
330  {
331  // +, +=
332  testPoseComposition(
333  x, y, z, yaw, pitch, roll, std_scale, x2, y2, z2, yaw2, pitch2,
334  roll2, std_scale2);
335  // -, -=, unary "-"
336  testPoseInverseComposition(
337  x, y, z, yaw, pitch, roll, std_scale, x2, y2, z2, yaw2, pitch2,
338  roll2, std_scale2);
339  // unitary "-" & ".inverse()"
340  testPoseInverse(x, y, z, yaw, pitch, roll, std_scale);
341  }
342 
343  void testChangeCoordsRef(
344  double x, double y, double z, double yaw, double pitch, double roll,
345  double std_scale, double x2, double y2, double z2, double yaw2,
346  double pitch2, double roll2)
347  {
348  CPose3DPDFGaussian p6pdf1 =
349  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
350 
351  const CPose3D new_base = CPose3D(x2, y2, z2, yaw2, pitch2, roll2);
352  const CPose3DPDFGaussian new_base_pdf(
353  new_base, CMatrixDouble66()); // COV = Zeros
354 
355  const CPose3DPDFGaussian p6_new_base_pdf = new_base_pdf + p6pdf1;
356  p6pdf1.changeCoordinatesReference(new_base);
357 
358  // Compare:
359  EXPECT_NEAR(
360  0, (p6_new_base_pdf.cov - p6pdf1.cov).array().abs().mean(), 1e-2)
361  << "p1 mean: " << p6pdf1.mean << endl
362  << "new_base: " << new_base << endl;
363  EXPECT_NEAR(
364  0, (p6_new_base_pdf.mean.getAsVectorVal() -
365  p6pdf1.mean.getAsVectorVal())
366  .array()
367  .abs()
368  .mean(),
369  1e-2)
370  << "p1 mean: " << p6pdf1.mean << endl
371  << "new_base: " << new_base << endl;
372  }
373 };
374 
375 TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
376 {
377  testToQuatPDFAndBack(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
378  testToQuatPDFAndBack(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.2);
379 
380  testToQuatPDFAndBack(6, -2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
381  testToQuatPDFAndBack(6, -2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.2);
382 
383  testToQuatPDFAndBack(6, -2, -3, DEG2RAD(10), DEG2RAD(40), DEG2RAD(5), 0.1);
384  testToQuatPDFAndBack(6, -2, -3, DEG2RAD(10), DEG2RAD(40), DEG2RAD(5), 0.2);
385 
386  testToQuatPDFAndBack(
387  6, -2, -3, DEG2RAD(-50), DEG2RAD(87), DEG2RAD(20), 0.1);
388  testToQuatPDFAndBack(
389  6, -2, -3, DEG2RAD(-50), DEG2RAD(87), DEG2RAD(20), 0.2);
390 
391  testToQuatPDFAndBack(
392  6, -2, -3, DEG2RAD(-50), DEG2RAD(-87), DEG2RAD(20), 0.1);
393  testToQuatPDFAndBack(
394  6, -2, -3, DEG2RAD(-50), DEG2RAD(-87), DEG2RAD(20), 0.2);
395 }
396 
397 TEST_F(Pose3DPDFGaussTests, CompositionJacobian)
398 {
399  testCompositionJacobian(
400  0, 0, 0, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0),
401  DEG2RAD(0), DEG2RAD(0));
402  testCompositionJacobian(
403  0, 0, 0, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, 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(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
410  DEG2RAD(0), DEG2RAD(0));
411  testCompositionJacobian(
412  1, 2, -3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
413  DEG2RAD(0), DEG2RAD(0));
414  testCompositionJacobian(
415  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
416  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,
422  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
423  testCompositionJacobian(
424  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10,
425  DEG2RAD(-50), DEG2RAD(-10), DEG2RAD(30));
426  testCompositionJacobian(
427  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
428  DEG2RAD(10), DEG2RAD(30));
429  testCompositionJacobian(
430  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
431  DEG2RAD(-10), DEG2RAD(-30));
432 }
433 
434 // Test the +, -, +=, -=, "-" operators
435 TEST_F(Pose3DPDFGaussTests, AllOperators)
436 {
437  testAllPoseOperators(
438  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
439  DEG2RAD(0), DEG2RAD(0), 0.1);
440  testAllPoseOperators(
441  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
442  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
443 
444  testAllPoseOperators(
445  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
446  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
447  testAllPoseOperators(
448  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
449  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
450 
451  testAllPoseOperators(
452  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
453  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
454  testAllPoseOperators(
455  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
456  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
457  testAllPoseOperators(
458  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
459  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
460  testAllPoseOperators(
461  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
462  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
463  testAllPoseOperators(
464  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
465  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
466  testAllPoseOperators(
467  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
468  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
469 }
470 
471 TEST_F(Pose3DPDFGaussTests, ChangeCoordsRef)
472 {
473  testChangeCoordsRef(
474  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
475  DEG2RAD(0), DEG2RAD(0));
476  testChangeCoordsRef(
477  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
478  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
479 
480  testChangeCoordsRef(
481  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
482  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
483  testChangeCoordsRef(
484  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
485  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
486 }
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:79
z
GLdouble GLdouble z
Definition: glext.h:3872
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:326
Pose3DPDFGaussTests::func_compose
static void func_compose(const CArrayDouble< 12 > &x, const double &dummy, CArrayDouble< 6 > &Y)
Definition: CPose3DPDFGaussian_unittest.cpp:70
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:263
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42
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:141
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:174
mrpt::math::CArrayNumeric
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
mrpt::math::CMatrixDouble66
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
Definition: eigen_frwds.h:59
TEST_F
TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
Definition: CPose3DPDFGaussian_unittest.cpp:375
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:153
Pose3DPDFGaussTests::testPoseInverse
void testPoseInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DPDFGaussian_unittest.cpp:275
mrpt::poses::CPose3DQuatPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
Definition: CPose3DQuatPDFGaussian.h:44
Pose3DPDFGaussTests::func_inv_compose
static void func_inv_compose(const CArrayDouble< 2 *6 > &x, const double &dummy, CArrayDouble< 6 > &Y)
Definition: CPose3DPDFGaussian_unittest.cpp:81
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:29
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:40
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:416
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:209
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:148
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
CPose3DPDFGaussian.h
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:83
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:343
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
transform_gaussian.h
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
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::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:40
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::math::cov
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
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:214
mrpt::poses::CPose3DPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CPose3DPDFGaussian.cpp:303
x
GLenum GLint x
Definition: glext.h:3538
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:19
p
GLfloat GLfloat p
Definition: glext.h:6305
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:93
CPose3DQuatPDFGaussian.h
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186


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