MRPT  1.9.9
CPose3DQuatPDFGaussian_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-2019, 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/num_jacobian.h>
14 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/random.h>
18 #include <Eigen/Dense>
19 
20 using namespace mrpt;
21 using namespace mrpt::poses;
22 using namespace mrpt::math;
23 using namespace std;
24 
25 template class mrpt::CTraitsTest<CPose3DQuatPDFGaussian>;
26 
27 class Pose3DQuatPDFGaussTests : public ::testing::Test
28 {
29  protected:
30  void SetUp() override {}
31  void TearDown() override {}
33  double x, double y, double z, double yaw, double pitch, double roll,
34  double std_scale)
35  {
37  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale));
38  }
39 
41  double x, double y, double z, double yaw, double pitch, double roll,
42  double std_scale)
43  {
46  r, 0, std_scale);
48  cov.matProductOf_AAt(r); // random semi-definite positive matrix:
49  for (int i = 0; i < 6; i++) cov(i, i) += 1e-7;
50  CPose3DPDFGaussian p6pdf(CPose3D(x, y, z, yaw, pitch, roll), cov);
51  return p6pdf;
52  }
53 
54  void test_toFromYPRGauss(double yaw, double pitch, double roll)
55  {
56  // Random pose:
57  CPose3DPDFGaussian p1ypr =
58  generateRandomPose3DPDF(1.0, 2.0, 3.0, yaw, pitch, roll, 0.1);
60 
61  // Convert back to a 6x6 representation:
62  CPose3DPDFGaussian p2ypr = CPose3DPDFGaussian(p1quat);
63 
64  EXPECT_NEAR(0, (p2ypr.cov - p1ypr.cov).array().abs().mean(), 1e-2)
65  << "p1ypr: " << endl
66  << p1ypr << endl
67  << "p1quat : " << endl
68  << p1quat << endl
69  << "p2ypr : " << endl
70  << p2ypr << endl;
71  }
72 
73  static void func_compose(
74  const CVectorFixedDouble<2 * 7>& x, const double& dummy,
76  {
77  MRPT_UNUSED_PARAM(dummy);
78  const CPose3DQuat p1(
79  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
80  const CPose3DQuat p2(
81  x[7 + 0], x[7 + 1], x[7 + 2],
82  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
83  const CPose3DQuat p = p1 + p2;
84  for (int i = 0; i < 7; i++) Y[i] = p[i];
85  }
86 
87  static void func_inv_compose(
88  const CVectorFixedDouble<2 * 7>& x, const double& dummy,
90  {
91  MRPT_UNUSED_PARAM(dummy);
92  const CPose3DQuat p1(
93  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
94  const CPose3DQuat p2(
95  x[7 + 0], x[7 + 1], x[7 + 2],
96  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
97  const CPose3DQuat p = p1 - p2;
98  for (int i = 0; i < 7; i++) Y[i] = p[i];
99  }
100 
102  double x, double y, double z, double yaw, double pitch, double roll,
103  double std_scale, double x2, double y2, double z2, double yaw2,
104  double pitch2, double roll2, double std_scale2)
105  {
106  CPose3DQuatPDFGaussian p7pdf1 =
107  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
108  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
109  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
110 
111  CPose3DQuatPDFGaussian p7_comp = p7pdf1 + p7pdf2;
112 
113  // Numeric approximation:
114  CVectorFixedDouble<7> y_mean;
116  {
118  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
119  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
120 
122  x_cov.insertMatrix(0, 0, p7pdf1.cov);
123  x_cov.insertMatrix(7, 7, p7pdf2.cov);
124 
125  double DUMMY = 0;
127  x_incrs.fill(1e-6);
129  x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
130  }
131  // Compare:
132  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
133  << "p1 mean: " << p7pdf1.mean << endl
134  << "p2 mean: " << p7pdf2.mean << endl
135  << "Numeric approximation of covariance: " << endl
136  << y_cov << endl
137  << "Returned covariance: " << endl
138  << p7_comp.cov << endl;
139  }
140 
141  static void func_inverse(
142  const CVectorFixedDouble<7>& x, const double& dummy,
144  {
145  MRPT_UNUSED_PARAM(dummy);
146  const CPose3DQuat p1(
147  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
148  const CPose3DQuat p1_inv(-p1);
149  for (int i = 0; i < 7; i++) Y[i] = p1_inv[i];
150  }
151 
153  double x, double y, double z, double yaw, double pitch, double roll,
154  double x2, double y2, double z2, double yaw2, double pitch2,
155  double roll2)
156  {
157  const CPose3DQuat q1(CPose3D(x, y, z, yaw, pitch, roll));
158  const CPose3DQuat q2(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
159 
160  // Theoretical Jacobians:
162  df_du(UNINITIALIZED_MATRIX);
164  q1, // x
165  q2, // u
166  df_dx, df_du);
167 
168  // Numerical approximation:
170  num_df_du(UNINITIALIZED_MATRIX);
171  {
173  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
174  for (int i = 0; i < 7; i++) x_mean[7 + i] = q2[i];
175 
176  double DUMMY = 0;
178  x_incrs.fill(1e-7);
179  CMatrixDouble numJacobs;
181  x_mean,
182  std::function<void(
183  const CVectorFixedDouble<2 * 7>& x, const double& dummy,
184  CVectorFixedDouble<7>& Y)>(&func_compose),
185  x_incrs, DUMMY, numJacobs);
186 
187  num_df_dx = numJacobs.block<7, 7>(0, 0);
188  num_df_du = numJacobs.block<7, 7>(0, 7);
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 
214  double x, double y, double z, double yaw, double pitch, double roll,
215  double std_scale)
216  {
217  CPose3DQuatPDFGaussian p7pdf1 =
218  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
219 
220  CPose3DQuatPDFGaussian p7_inv = -p7pdf1;
221 
222  // Numeric approximation:
223  CVectorFixedDouble<7> y_mean;
225  {
226  CVectorFixedDouble<7> x_mean;
227  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
228 
230  x_cov.insertMatrix(0, 0, p7pdf1.cov);
231 
232  double DUMMY = 0;
233  CVectorFixedDouble<7> x_incrs;
234  x_incrs.fill(1e-6);
236  x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
237  }
238 
239  // Compare:
240  EXPECT_NEAR(0, (y_cov - p7_inv.cov).array().abs().mean(), 1e-2)
241  << "p1 mean: " << p7pdf1.mean << endl
242  << "inv mean: " << p7_inv.mean << endl
243  << "Numeric approximation of covariance: " << endl
244  << y_cov << endl
245  << "Returned covariance: " << endl
246  << p7_inv.cov << endl
247  << "Error: " << endl
248  << y_cov - p7_inv.cov << endl;
249  }
250 
252  double x, double y, double z, double yaw, double pitch, double roll,
253  double std_scale, double x2, double y2, double z2, double yaw2,
254  double pitch2, double roll2, double std_scale2)
255  {
256  CPose3DQuatPDFGaussian p7pdf1 =
257  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
258  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
259  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
260 
261  CPose3DQuatPDFGaussian p7_comp = p7pdf1 - p7pdf2;
262 
263  // Numeric approximation:
264  CVectorFixedDouble<7> y_mean;
266  {
268  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
269  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
270 
272  x_cov.insertMatrix(0, 0, p7pdf1.cov);
273  x_cov.insertMatrix(7, 7, p7pdf2.cov);
274 
275  double DUMMY = 0;
277  x_incrs.fill(1e-6);
279  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
280  }
281  // Compare:
282  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
283  << "p1 mean: " << p7pdf1.mean << endl
284  << "p2 mean: " << p7pdf2.mean << endl
285  << "Numeric approximation of covariance: " << endl
286  << y_cov << endl
287  << "Returned covariance: " << endl
288  << p7_comp.cov << endl;
289  }
290 
292  double x, double y, double z, double yaw, double pitch, double roll,
293  double std_scale, double x2, double y2, double z2, double yaw2,
294  double pitch2, double roll2)
295  {
296  CPose3DQuatPDFGaussian p7pdf1 =
297  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
298 
299  const CPose3DQuat new_base =
300  CPose3DQuat(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
301  const CPose3DQuatPDFGaussian new_base_pdf(
302  new_base, CMatrixDouble77()); // COV = Zeros
303 
304  const CPose3DQuatPDFGaussian p7_new_base_pdf = new_base_pdf + p7pdf1;
305  p7pdf1.changeCoordinatesReference(new_base);
306 
307  // Compare:
308  EXPECT_NEAR(
309  0, (p7_new_base_pdf.cov - p7pdf1.cov).array().abs().mean(), 1e-2)
310  << "p1 mean: " << p7pdf1.mean << endl
311  << "new_base: " << new_base << endl;
312  EXPECT_NEAR(
313  0,
314  (p7_new_base_pdf.mean.asVectorVal() - p7pdf1.mean.asVectorVal())
315  .array()
316  .abs()
317  .mean(),
318  1e-2)
319  << "p1 mean: " << p7pdf1.mean << endl
320  << "new_base: " << new_base << endl;
321  }
322 };
323 
324 TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
325 {
326  test_toFromYPRGauss(-30.0_deg, 10.0_deg, 60.0_deg);
327  test_toFromYPRGauss(30.0_deg, 88.0_deg, 0.0_deg);
328  test_toFromYPRGauss(30.0_deg, 89.5_deg, 0.0_deg);
329  // The formulas break at pitch=90, but this we cannot avoid...
330 }
331 
332 TEST_F(Pose3DQuatPDFGaussTests, CompositionJacobian)
333 {
334  testCompositionJacobian(
335  0, 0, 0, 2.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
336  testCompositionJacobian(
337  1, 2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
338  0.0_deg);
339  testCompositionJacobian(
340  1, -2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
341  0.0_deg);
342  testCompositionJacobian(
343  1, 2, -3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
344  0.0_deg);
345  testCompositionJacobian(
346  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
347  30.0_deg);
348  testCompositionJacobian(
349  1, 2, 3, 20.0_deg, -80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
350  30.0_deg);
351  testCompositionJacobian(
352  1, 2, 3, 20.0_deg, 80.0_deg, -70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
353  30.0_deg);
354  testCompositionJacobian(
355  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, -50.0_deg, -10.0_deg,
356  30.0_deg);
357  testCompositionJacobian(
358  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, 10.0_deg,
359  30.0_deg);
360  testCompositionJacobian(
361  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
362  -30.0_deg);
363 }
364 
366 {
367  testInverse(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
368  testInverse(0, 0, 0, 10.0_deg, 0.0_deg, 0.0_deg, 0.1);
369  testInverse(0, 0, 0, 0.0_deg, 10.0_deg, 0.0_deg, 0.1);
370  testInverse(0, 0, 0, 0.0_deg, 0.0_deg, 10.0_deg, 0.1);
371 
372  testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
373  testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.2);
374 
375  testInverse(1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
376  testInverse(-1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
377  testInverse(1, 2, -3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
378  testInverse(-1, 2, -3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
379  testInverse(1, 2, 3, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
380  testInverse(-1, 2, 3, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
381  testInverse(1, 2, -3, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
382  testInverse(-1, 2, -3, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
383  testInverse(1, 2, 3, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
384  testInverse(-1, 2, 3, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
385  testInverse(1, 2, -3, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
386  testInverse(-1, 2, -3, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
387  testInverse(1, 2, 3, 0.0_deg, -30.0_deg, 0.0_deg, 0.1);
388  testInverse(-1, 2, 3, 0.0_deg, -30.0_deg, 0.0_deg, 0.1);
389  testInverse(1, 2, -3, 0.0_deg, -30.0_deg, 0.0_deg, 0.1);
390  testInverse(-1, 2, -3, 0.0_deg, -30.0_deg, 0.0_deg, 0.1);
391  testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 30.0_deg, 0.1);
392  testInverse(-1, 2, 3, 0.0_deg, 0.0_deg, 30.0_deg, 0.1);
393  testInverse(1, 2, -3, 0.0_deg, 0.0_deg, 30.0_deg, 0.1);
394  testInverse(-1, 2, -3, 0.0_deg, 0.0_deg, 30.0_deg, 0.1);
395  testInverse(1, 2, 3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
396  testInverse(-1, 2, 3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
397  testInverse(1, 2, -3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
398  testInverse(-1, 2, -3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
399 }
400 
402 {
403  testPoseComposition(
404  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
405  0.0_deg, 0.1);
406  testPoseComposition(
407  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
408  0.0_deg, 0.1);
409 
410  testPoseComposition(
411  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
412  -10.0_deg, 30.0_deg, 0.1);
413  testPoseComposition(
414  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
415  -10.0_deg, 30.0_deg, 0.2);
416 
417  testPoseComposition(
418  1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
419  0.0_deg, 0.1);
420  testPoseComposition(
421  1, 2, 3, 0.0_deg, 10.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
422  0.0_deg, 0.1);
423  testPoseComposition(
424  1, 2, 3, 0.0_deg, 0.0_deg, 10.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
425  0.0_deg, 0.1);
426  testPoseComposition(
427  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 10.0_deg, 0.0_deg,
428  0.0_deg, 0.1);
429  testPoseComposition(
430  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 10.0_deg,
431  0.0_deg, 0.1);
432  testPoseComposition(
433  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
434  10.0_deg, 0.1);
435 }
436 
437 TEST_F(Pose3DQuatPDFGaussTests, InverseComposition)
438 {
439  testPoseInverseComposition(
440  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
441  0.0_deg, 0.1);
442  testPoseInverseComposition(
443  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
444  0.0_deg, 0.1);
445 
446  testPoseInverseComposition(
447  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
448  -10.0_deg, 30.0_deg, 0.1);
449  testPoseInverseComposition(
450  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
451  -10.0_deg, 30.0_deg, 0.2);
452 
453  testPoseInverseComposition(
454  1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
455  0.0_deg, 0.1);
456  testPoseInverseComposition(
457  1, 2, 3, 0.0_deg, 10.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
458  0.0_deg, 0.1);
459  testPoseInverseComposition(
460  1, 2, 3, 0.0_deg, 0.0_deg, 10.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
461  0.0_deg, 0.1);
462  testPoseInverseComposition(
463  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 10.0_deg, 0.0_deg,
464  0.0_deg, 0.1);
465  testPoseInverseComposition(
466  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 10.0_deg,
467  0.0_deg, 0.1);
468  testPoseInverseComposition(
469  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
470  10.0_deg, 0.1);
471 }
472 
474 {
475  testChangeCoordsRef(
476  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
477  0.0_deg);
478  testChangeCoordsRef(
479  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
480  0.0_deg);
481 
482  testChangeCoordsRef(
483  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
484  -10.0_deg, 30.0_deg);
485  testChangeCoordsRef(
486  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
487  -10.0_deg, 30.0_deg);
488 }
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
GLdouble GLdouble z
Definition: glext.h:3879
static CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
void test_toFromYPRGauss(double yaw, double pitch, double roll)
TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
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
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:505
STL namespace.
CMatrixFixed< double, 7, 7 > CMatrixDouble77
Definition: CMatrixFixed.h:355
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.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
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
static void func_inv_compose(const CVectorFixedDouble< 2 *7 > &x, const double &dummy, CVectorFixedDouble< 7 > &Y)
void testInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static void func_inverse(const CVectorFixedDouble< 7 > &x, const double &dummy, CVectorFixedDouble< 7 > &Y)
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
static void func_compose(const CVectorFixedDouble< 2 *7 > &x, const double &dummy, CVectorFixedDouble< 7 > &Y)
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)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
GLenum GLint GLint y
Definition: glext.h:3542
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)
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
GLenum GLint x
Definition: glext.h:3542
static void jacobiansPoseComposition(const CPose3DQuat &x, const CPose3DQuat &u, mrpt::math::CMatrixDouble77 &df_dx, mrpt::math::CMatrixDouble77 &df_du, CPose3DQuat *out_x_oplus_u=nullptr)
This static method computes the two Jacobians of a pose composition operation $f(x,u)= x u$.
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)
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLfloat GLfloat p
Definition: glext.h:6398
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++)
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
static CPose3DQuatPDFGaussian generateRandomPoseQuat3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019