MRPT  1.9.9
CPose3DPDFGaussian_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-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 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
CPose3D mean
The mean value.
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)
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
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
void drawGaussian1DMatrix(MAT &matrix, const double mean=0, const double std=1)
Fills the given matrix with independent, 1D-normally distributed samples.
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(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
STL namespace.
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)
void testPoseInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
This base provides a set of functions for maths stuff.
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
static void func_compose(const CVectorFixedDouble< 12 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 6 > &Y)
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...
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
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
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)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static void func_inv_compose(const CVectorFixedDouble< 2 *6 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 6 > &Y)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
static CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
void testToQuatPDFAndBack(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
CMatrixFixed< double, 6, 6 > CMatrixDouble66
Definition: CMatrixFixed.h:369
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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)
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
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)
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
for(unsigned int i=0;i< NUM_IMGS;i++)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020