Main MRPT website > C++ reference for 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>
13 #include <mrpt/random.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() {}
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.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 
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.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
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;
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 
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);
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 
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:
163  df_du(UNINITIALIZED_MATRIX);
165  q1, // x
166  q2, // u
167  df_dx, df_du);
168 
169  // Numerical approximation:
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;
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
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;
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 
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);
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
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
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 
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
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 }
Pose3DPDFGaussTests::func_compose
static void func_compose(const CArrayDouble< 12 > &x, const double &dummy, CArrayDouble< 6 > &Y)
Definition: CPose3DPDFGaussian_unittest.cpp:70
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
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:40
TEST_F
TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
Definition: CPose3DPDFGaussian_unittest.cpp:375
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
Pose3DPDFGaussTests::TearDown
virtual void TearDown()
Definition: CPose3DPDFGaussian_unittest.cpp:30
Pose3DPDFGaussTests::SetUp
virtual void SetUp()
Definition: CPose3DPDFGaussian_unittest.cpp:29
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
CPose3DPDFGaussian.h
mrpt::obs::gnss::roll
double roll
Definition: gnss_messages_novatel.h:264
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
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
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
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
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
random.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
mrpt::math::CMatrixTemplateNumeric< double >
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
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::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:150
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
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::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::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
Pose3DPDFGaussTests
Definition: CPose3DPDFGaussian_unittest.cpp:26
mrpt::math::CArrayNumeric
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
Definition: CArrayNumeric.h:25
mrpt::poses::CPose3DPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CPose3DPDFGaussian.cpp:303
mrpt::obs::gnss::pitch
double pitch
Definition: gnss_messages_novatel.h:264
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:40
mrpt::math::CMatrixDouble66
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
Definition: eigen_frwds.h:59
CPose3D.h
mrpt::math::UNINITIALIZED_MATRIX
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
CPose3DQuatPDFGaussian.h
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:19
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
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
mrpt::poses::CPose3DQuatPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
Definition: CPose3DQuatPDFGaussian.h:44
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
z
GLdouble GLdouble z
Definition: glext.h:3872
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:83
transform_gaussian.h
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:79
Pose3DPDFGaussTests::testToQuatPDFAndBack
void testToQuatPDFAndBack(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DPDFGaussian_unittest.cpp:45
y
GLenum GLint GLint y
Definition: glext.h:3538
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::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
x
GLenum GLint x
Definition: glext.h:3538
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST