MRPT  2.0.4
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-2020, 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, [[maybe_unused]] const double& dummy,
70  {
71  const CPose3D p1(x[0], x[1], x[2], x[3], x[4], x[5]);
72  const CPose3D p2(
73  x[6 + 0], x[6 + 1], x[6 + 2], x[6 + 3], x[6 + 4], x[6 + 5]);
74  const CPose3D p = p1 + p2;
75  for (int i = 0; i < 6; i++) Y[i] = p[i];
76  }
77 
78  static void func_inv_compose(
80  [[maybe_unused]] const double& dummy, CVectorFixedDouble<6>& Y)
81  {
82  const CPose3D p1(x[0], x[1], x[2], x[3], x[4], x[5]);
83  const CPose3D p2(
84  x[6 + 0], x[6 + 1], x[6 + 2], x[6 + 3], x[6 + 4], x[6 + 5]);
85  const CPose3D p = p1 - p2;
86  for (int i = 0; i < 6; i++) Y[i] = p[i];
87  }
88 
89  // Test "+" & "+=" operator
91  double x, double y, double z, double yaw, double pitch, double roll,
92  double std_scale, double x2, double y2, double z2, double yaw2,
93  double pitch2, double roll2, double std_scale2)
94  {
95  CPose3DPDFGaussian p6pdf1 =
96  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
97  CPose3DPDFGaussian p6pdf2 = generateRandomPose3DPDF(
98  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
99 
100  // With "+" operators:
101  CPose3DPDFGaussian p6_comp = p6pdf1 + p6pdf2;
102 
103  // Numeric approximation:
104  CVectorFixedDouble<6> y_mean;
106  {
107  CVectorFixedDouble<12> x_mean;
108  for (int i = 0; i < 6; i++) x_mean[i] = p6pdf1.mean[i];
109  for (int i = 0; i < 6; i++) x_mean[6 + i] = p6pdf2.mean[i];
110 
112  x_cov.insertMatrix(0, 0, p6pdf1.cov);
113  x_cov.insertMatrix(6, 6, p6pdf2.cov);
114 
115  double DUMMY = 0;
116  CVectorFixedDouble<12> x_incrs;
117  x_incrs.fill(1e-6);
119  x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
120  }
121  // Compare mean:
122  EXPECT_NEAR(0, (y_mean - p6_comp.mean.asVectorVal()).sum_abs(), 1e-2)
123  << "p1 mean: " << p6pdf1.mean << endl
124  << "p2 mean: " << p6pdf2.mean << endl;
125 
126  // Compare cov:
127  EXPECT_NEAR(0, (y_cov - p6_comp.cov).sum_abs(), 1e-2)
128  << "p1 mean: " << p6pdf1.mean << endl
129  << "p2 mean: " << p6pdf2.mean << endl;
130 
131  // Test +=
132  p6_comp = p6pdf1;
133  p6_comp += p6pdf2;
134 
135  // Compare mean:
136  EXPECT_NEAR(0, (y_mean - p6_comp.mean.asVectorVal()).sum_abs(), 1e-2)
137  << "p1 mean: " << p6pdf1.mean << endl
138  << "p2 mean: " << p6pdf2.mean << endl;
139 
140  // Compare cov:
141  EXPECT_NEAR(0, (y_cov - p6_comp.cov).sum_abs(), 1e-2)
142  << "p1 mean: " << p6pdf1.mean << endl
143  << "p2 mean: " << p6pdf2.mean << endl;
144  }
145 
147  double x, double y, double z, double yaw, double pitch, double roll,
148  double x2, double y2, double z2, double yaw2, double pitch2,
149  double roll2)
150  {
151  const CPose3D q1(x, y, z, yaw, pitch, roll);
152  const CPose3D q2(x2, y2, z2, yaw2, pitch2, roll2);
153 
154  // Theoretical Jacobians:
156  df_du(UNINITIALIZED_MATRIX);
158  q1, // x
159  q2, // u
160  df_dx, df_du);
161 
162  // Numerical approximation:
164  num_df_du(UNINITIALIZED_MATRIX);
165  {
167  for (int i = 0; i < 6; i++) x_mean[i] = q1[i];
168  for (int i = 0; i < 6; i++) x_mean[6 + i] = q2[i];
169 
170  double DUMMY = 0;
172  x_incrs.fill(1e-7);
173  CMatrixDouble numJacobs;
175  x_mean,
176  std::function<void(
177  const CVectorFixedDouble<12>& x, const double& dummy,
178  CVectorFixedDouble<6>& Y)>(&func_compose),
179  x_incrs, DUMMY, numJacobs);
180 
181  num_df_dx = numJacobs.block<6, 6>(0, 0);
182  num_df_du = numJacobs.block<6, 6>(0, 6);
183  }
184 
185  // Compare:
186  EXPECT_NEAR(0, (df_dx - num_df_dx).sum_abs(), 3e-3)
187  << "q1: " << q1 << endl
188  << "q2: " << q2 << endl
189  << "Numeric approximation of df_dx: " << endl
190  << num_df_dx << endl
191  << "Implemented method: " << endl
192  << df_dx << endl
193  << "Error: " << endl
194  << df_dx - num_df_dx << endl;
195 
196  EXPECT_NEAR(0, (df_du - num_df_du).sum_abs(), 3e-3)
197  << "q1: " << q1 << endl
198  << "q2: " << q2 << endl
199  << "Numeric approximation of df_du: " << endl
200  << num_df_du << endl
201  << "Implemented method: " << endl
202  << df_du << endl
203  << "Error: " << endl
204  << df_du - num_df_du << endl;
205  }
206 
207  // Test the "-" & "-=" operator
209  double x, double y, double z, double yaw, double pitch, double roll,
210  double std_scale, double x2, double y2, double z2, double yaw2,
211  double pitch2, double roll2, double std_scale2)
212  {
213  CPose3DPDFGaussian p6pdf1 =
214  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
215  CPose3DPDFGaussian p6pdf2 = generateRandomPose3DPDF(
216  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
217 
218  // With the "-" operator
219  CPose3DPDFGaussian p6_comp = p6pdf1 - p6pdf2;
220 
221  // Numeric approximation:
222  CVectorFixedDouble<6> y_mean;
224  {
226  for (int i = 0; i < 6; i++) x_mean[i] = p6pdf1.mean[i];
227  for (int i = 0; i < 6; i++) x_mean[6 + i] = p6pdf2.mean[i];
228 
230  x_cov.insertMatrix(0, 0, p6pdf1.cov);
231  x_cov.insertMatrix(6, 6, p6pdf2.cov);
232 
233  double DUMMY = 0;
235  x_incrs.fill(1e-6);
237  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
238  }
239  // Compare mean:
240  EXPECT_NEAR(0, (y_mean - p6_comp.mean.asVectorVal()).sum_abs(), 1e-2)
241  << "p1 mean: " << p6pdf1.mean << endl
242  << "p2 mean: " << p6pdf2.mean << endl;
243 
244  // Compare cov:
245  EXPECT_NEAR(0, (y_cov - p6_comp.cov).sum_abs(), 1e-2)
246  << "p1 mean: " << p6pdf1.mean << endl
247  << "p2 mean: " << p6pdf2.mean << endl;
248 
249  // With the "-=" operator
250  p6_comp = p6pdf1;
251  p6_comp -= p6pdf2;
252 
253  // Compare mean:
254  EXPECT_NEAR(0, (y_mean - p6_comp.mean.asVectorVal()).sum_abs(), 1e-2)
255  << "p1 mean: " << p6pdf1.mean << endl
256  << "p2 mean: " << p6pdf2.mean << endl;
257 
258  // Compare cov:
259  EXPECT_NEAR(0, (y_cov - p6_comp.cov).sum_abs(), 1e-2)
260  << "p1 mean: " << p6pdf1.mean << endl
261  << "p2 mean: " << p6pdf2.mean << endl;
262  }
263 
264  // Test the unary "-" operator
266  double x, double y, double z, double yaw, double pitch, double roll,
267  double std_scale)
268  {
269  CPose3DPDFGaussian p6pdf2 =
270  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
271  CPose3DPDFGaussian p6_zero(
272  CPose3D(0, 0, 0, 0, 0, 0), CMatrixDouble66()); // COV=All zeros
273 
274  // Unary "-":
275  const CPose3DPDFGaussian p6_inv = -p6pdf2;
276 
277  // Compare to the binary "-" operator:
278  const CPose3DPDFGaussian p6_comp = p6_zero - p6pdf2;
279 
280  // Compare mean:
281  EXPECT_NEAR(
282  0,
283  (p6_inv.mean.asVectorVal() - p6_comp.mean.asVectorVal())
284  .array()
285  .abs()
286  .sum(),
287  1e-2)
288  << "p mean: " << p6pdf2.mean << endl;
289 
290  // Compare cov:
291  EXPECT_NEAR(0, (p6_inv.cov - p6_comp.cov).sum_abs(), 1e-2)
292  << "p mean: " << p6pdf2.mean << endl;
293 
294  // Compare to the "inverse()" method:
295  CPose3DPDFGaussian p6_inv2;
296  p6pdf2.inverse(p6_inv2);
297 
298  // Compare mean:
299  EXPECT_NEAR(
300  0,
301  (p6_inv2.mean.asVectorVal() - p6_comp.mean.asVectorVal())
302  .array()
303  .abs()
304  .sum(),
305  1e-2)
306  << "p mean: " << p6pdf2.mean << endl
307  << "p6_inv2 mean: " << p6_inv2.mean << endl
308  << "p6_comp mean: " << p6_comp.mean << endl;
309 
310  // Compare cov:
311  EXPECT_NEAR(0, (p6_inv2.cov - p6_comp.cov).sum_abs(), 1e-2)
312  << "p mean: " << p6pdf2.mean << endl
313  << "p6_inv2 mean: " << p6_inv2.mean << endl
314  << "p6_comp mean: " << p6_comp.mean << endl;
315  }
316 
317  // Test all operators
319  double x, double y, double z, double yaw, double pitch, double roll,
320  double std_scale, double x2, double y2, double z2, double yaw2,
321  double pitch2, double roll2, double std_scale2)
322  {
323  // +, +=
324  testPoseComposition(
325  x, y, z, yaw, pitch, roll, std_scale, x2, y2, z2, yaw2, pitch2,
326  roll2, std_scale2);
327  // -, -=, unary "-"
328  testPoseInverseComposition(
329  x, y, z, yaw, pitch, roll, std_scale, x2, y2, z2, yaw2, pitch2,
330  roll2, std_scale2);
331  // unitary "-" & ".inverse()"
332  testPoseInverse(x, y, z, yaw, pitch, roll, std_scale);
333  }
334 
336  double x, double y, double z, double yaw, double pitch, double roll,
337  double std_scale, double x2, double y2, double z2, double yaw2,
338  double pitch2, double roll2)
339  {
340  CPose3DPDFGaussian p6pdf1 =
341  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale);
342 
343  const CPose3D new_base = CPose3D(x2, y2, z2, yaw2, pitch2, roll2);
344  const CPose3DPDFGaussian new_base_pdf(
345  new_base, CMatrixDouble66()); // COV = Zeros
346 
347  const CPose3DPDFGaussian p6_new_base_pdf = new_base_pdf + p6pdf1;
348  p6pdf1.changeCoordinatesReference(new_base);
349 
350  // Compare:
351  EXPECT_NEAR(
352  0, (p6_new_base_pdf.cov - p6pdf1.cov).array().abs().mean(), 1e-2)
353  << "p1 mean: " << p6pdf1.mean << endl
354  << "new_base: " << new_base << endl;
355  EXPECT_NEAR(
356  0,
357  (p6_new_base_pdf.mean.asVectorVal() - p6pdf1.mean.asVectorVal())
358  .array()
359  .abs()
360  .mean(),
361  1e-2)
362  << "p1 mean: " << p6pdf1.mean << endl
363  << "new_base: " << new_base << endl;
364  }
365 };
366 
367 TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
368 {
369  testToQuatPDFAndBack(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
370  testToQuatPDFAndBack(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.2);
371 
372  testToQuatPDFAndBack(6, -2, -3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
373  testToQuatPDFAndBack(6, -2, -3, 0.0_deg, 0.0_deg, 0.0_deg, 0.2);
374 
375  testToQuatPDFAndBack(6, -2, -3, 10.0_deg, 40.0_deg, 5.0_deg, 0.1);
376  testToQuatPDFAndBack(6, -2, -3, 10.0_deg, 40.0_deg, 5.0_deg, 0.2);
377 
378  testToQuatPDFAndBack(6, -2, -3, -50.0_deg, 87.0_deg, 20.0_deg, 0.1);
379  testToQuatPDFAndBack(6, -2, -3, -50.0_deg, 87.0_deg, 20.0_deg, 0.2);
380 
381  testToQuatPDFAndBack(6, -2, -3, -50.0_deg, -87.0_deg, 20.0_deg, 0.1);
382  testToQuatPDFAndBack(6, -2, -3, -50.0_deg, -87.0_deg, 20.0_deg, 0.2);
383 }
384 
385 TEST_F(Pose3DPDFGaussTests, CompositionJacobian)
386 {
387  testCompositionJacobian(
388  0, 0, 0, 2.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
389  testCompositionJacobian(
390  0, 0, 0, 2.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
391  testCompositionJacobian(
392  1, 2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
393  0.0_deg);
394  testCompositionJacobian(
395  1, -2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
396  0.0_deg);
397  testCompositionJacobian(
398  1, 2, -3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
399  0.0_deg);
400  testCompositionJacobian(
401  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
402  30.0_deg);
403  testCompositionJacobian(
404  1, 2, 3, 20.0_deg, -80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
405  30.0_deg);
406  testCompositionJacobian(
407  1, 2, 3, 20.0_deg, 80.0_deg, -70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
408  30.0_deg);
409  testCompositionJacobian(
410  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, -50.0_deg, -10.0_deg,
411  30.0_deg);
412  testCompositionJacobian(
413  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, 10.0_deg,
414  30.0_deg);
415  testCompositionJacobian(
416  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
417  -30.0_deg);
418 }
419 
420 // Test the +, -, +=, -=, "-" operators
422 {
423  testAllPoseOperators(
424  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
425  0.0_deg, 0.1);
426  testAllPoseOperators(
427  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
428  0.0_deg, 0.1);
429 
430  testAllPoseOperators(
431  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
432  -10.0_deg, 30.0_deg, 0.1);
433  testAllPoseOperators(
434  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
435  -10.0_deg, 30.0_deg, 0.2);
436 
437  testAllPoseOperators(
438  1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
439  0.0_deg, 0.1);
440  testAllPoseOperators(
441  1, 2, 3, 0.0_deg, 10.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
442  0.0_deg, 0.1);
443  testAllPoseOperators(
444  1, 2, 3, 0.0_deg, 0.0_deg, 10.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
445  0.0_deg, 0.1);
446  testAllPoseOperators(
447  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 10.0_deg, 0.0_deg,
448  0.0_deg, 0.1);
449  testAllPoseOperators(
450  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 10.0_deg,
451  0.0_deg, 0.1);
452  testAllPoseOperators(
453  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
454  10.0_deg, 0.1);
455 }
456 
457 TEST_F(Pose3DPDFGaussTests, ChangeCoordsRef)
458 {
459  testChangeCoordsRef(
460  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
461  0.0_deg);
462  testChangeCoordsRef(
463  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
464  0.0_deg);
465 
466  testChangeCoordsRef(
467  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
468  -10.0_deg, 30.0_deg);
469  testChangeCoordsRef(
470  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
471  -10.0_deg, 30.0_deg);
472 }
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:90
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:149
Pose3DPDFGaussTests::SetUp
void SetUp() override
Definition: CPose3DPDFGaussian_unittest.cpp:29
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:39
TEST_F
TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
Definition: CPose3DPDFGaussian_unittest.cpp:367
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
EXPECT_TRUE
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
mrpt::math::MatrixVectorBase< T, CMatrixFixed< T, ROWS, COLS > >::fill
void fill(const T &val)
Definition: MatrixVectorBase.h:70
mrpt::math::CMatrixDouble66
CMatrixFixed< double, 6, 6 > CMatrixDouble66
Definition: CMatrixFixed.h:369
CPose3DPDFGaussian.h
mrpt::obs::gnss::roll
double roll
Definition: gnss_messages_novatel.h:306
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:318
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
Pose3DPDFGaussTests::TearDown
void TearDown() override
Definition: CPose3DPDFGaussian_unittest.cpp:30
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
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:146
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:335
Pose3DPDFGaussTests::testPoseInverse
void testPoseInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DPDFGaussian_unittest.cpp:265
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::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:408
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
Pose3DPDFGaussTests::func_inv_compose
static void func_inv_compose(const CVectorFixedDouble< 2 *6 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 6 > &Y)
Definition: CPose3DPDFGaussian_unittest.cpp:78
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::math::MatrixBase::matProductOf_AAt
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
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
Pose3DPDFGaussTests
Definition: CPose3DPDFGaussian_unittest.cpp:26
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
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
for
for(unsigned int i=0;i< NUM_IMGS;i++)
Definition: chessboard_stereo_camera_calib_unittest.cpp:33
mrpt::poses::CPose3DPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CPose3DPDFGaussian.cpp:297
mrpt::obs::gnss::pitch
double pitch
Definition: gnss_messages_novatel.h:306
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
CPose3D.h
mrpt::math::UNINITIALIZED_MATRIX
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:57
CPose3DQuatPDFGaussian.h
Pose3DPDFGaussTests::func_compose
static void func_compose(const CVectorFixedDouble< 12 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 6 > &Y)
Definition: CPose3DPDFGaussian_unittest.cpp:67
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
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::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
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:82
transform_gaussian.h
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:78
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::CMatrixDynamic< double >
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:208
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:206



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020