MRPT  2.0.0
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>
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, [[maybe_unused]] const double& dummy,
69  CVectorFixedDouble<6>& Y)
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(
79  const CVectorFixedDouble<2 * 6>& x,
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
90  void testPoseComposition(
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;
105  CMatrixFixed<double, 6, 6> y_cov;
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 
111  CMatrixFixed<double, 12, 12> x_cov;
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);
118  transform_gaussian_linear(
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 
146  void testCompositionJacobian(
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:
155  CMatrixDouble66 df_dx(UNINITIALIZED_MATRIX),
156  df_du(UNINITIALIZED_MATRIX);
157  CPose3DPDF::jacobiansPoseComposition(
158  q1, // x
159  q2, // u
160  df_dx, df_du);
161 
162  // Numerical approximation:
163  CMatrixDouble66 num_df_dx(UNINITIALIZED_MATRIX),
164  num_df_du(UNINITIALIZED_MATRIX);
165  {
166  CVectorFixedDouble<2 * 6> x_mean;
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;
171  CVectorFixedDouble<2 * 6> x_incrs;
172  x_incrs.fill(1e-7);
173  CMatrixDouble numJacobs;
174  mrpt::math::estimateJacobian(
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
208  void testPoseInverseComposition(
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;
223  CMatrixFixed<double, 6, 6> y_cov;
224  {
225  CVectorFixedDouble<2 * 6> x_mean;
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 
229  CMatrixFixed<double, 12, 12> x_cov;
230  x_cov.insertMatrix(0, 0, p6pdf1.cov);
231  x_cov.insertMatrix(6, 6, p6pdf2.cov);
232 
233  double DUMMY = 0;
234  CVectorFixedDouble<2 * 6> x_incrs;
235  x_incrs.fill(1e-6);
236  transform_gaussian_linear(
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
265  void testPoseInverse(
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
318  void testAllPoseOperators(
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 
335  void testChangeCoordsRef(
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
421 TEST_F(Pose3DPDFGaussTests, AllOperators)
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 }
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
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
EXPECT_TRUE
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
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:206
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:367
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:146
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::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:276
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:408
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
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::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:149
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:335
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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
transform_gaussian.h
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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:369
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:39
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::poses::CPose3DPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CPose3DPDFGaussian.cpp:297
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
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::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
for
for(unsigned int i=0;i< NUM_IMGS;i++)
Definition: chessboard_stereo_camera_calib_unittest.cpp:33


Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at mié abr 1 00:50:30 CEST 2020