MRPT  2.0.1
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-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/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(
75  [[maybe_unused]] const double& dummy, CVectorFixedDouble<7>& Y)
76  {
77  const CPose3DQuat p1(
78  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
79  const CPose3DQuat p2(
80  x[7 + 0], x[7 + 1], x[7 + 2],
81  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
82  const CPose3DQuat p = p1 + p2;
83  for (int i = 0; i < 7; i++) Y[i] = p[i];
84  }
85 
86  static void func_inv_compose(
88  [[maybe_unused]] const double& dummy, CVectorFixedDouble<7>& Y)
89  {
90  const CPose3DQuat p1(
91  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
92  const CPose3DQuat p2(
93  x[7 + 0], x[7 + 1], x[7 + 2],
94  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
95  const CPose3DQuat p = p1 - p2;
96  for (int i = 0; i < 7; i++) Y[i] = p[i];
97  }
98 
100  double x, double y, double z, double yaw, double pitch, double roll,
101  double std_scale, double x2, double y2, double z2, double yaw2,
102  double pitch2, double roll2, double std_scale2)
103  {
104  CPose3DQuatPDFGaussian p7pdf1 =
105  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
106  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
107  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
108 
109  CPose3DQuatPDFGaussian p7_comp = p7pdf1 + p7pdf2;
110 
111  // Numeric approximation:
112  CVectorFixedDouble<7> y_mean;
114  {
116  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
117  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
118 
120  x_cov.insertMatrix(0, 0, p7pdf1.cov);
121  x_cov.insertMatrix(7, 7, p7pdf2.cov);
122 
123  double DUMMY = 0;
125  x_incrs.fill(1e-6);
127  x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
128  }
129  // Compare:
130  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
131  << "p1 mean: " << p7pdf1.mean << endl
132  << "p2 mean: " << p7pdf2.mean << endl
133  << "Numeric approximation of covariance: " << endl
134  << y_cov << endl
135  << "Returned covariance: " << endl
136  << p7_comp.cov << endl;
137  }
138 
139  static void func_inverse(
140  const CVectorFixedDouble<7>& x, [[maybe_unused]] const double& dummy,
142  {
143  const CPose3DQuat p1(
144  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
145  const CPose3DQuat p1_inv(-p1);
146  for (int i = 0; i < 7; i++) Y[i] = p1_inv[i];
147  }
148 
150  double x, double y, double z, double yaw, double pitch, double roll,
151  double x2, double y2, double z2, double yaw2, double pitch2,
152  double roll2)
153  {
154  const CPose3DQuat q1(CPose3D(x, y, z, yaw, pitch, roll));
155  const CPose3DQuat q2(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
156 
157  // Theoretical Jacobians:
159  df_du(UNINITIALIZED_MATRIX);
161  q1, // x
162  q2, // u
163  df_dx, df_du);
164 
165  // Numerical approximation:
167  num_df_du(UNINITIALIZED_MATRIX);
168  {
170  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
171  for (int i = 0; i < 7; i++) x_mean[7 + i] = q2[i];
172 
173  double DUMMY = 0;
175  x_incrs.fill(1e-7);
176  CMatrixDouble numJacobs;
178  x_mean,
179  std::function<void(
180  const CVectorFixedDouble<2 * 7>& x, const double& dummy,
181  CVectorFixedDouble<7>& Y)>(&func_compose),
182  x_incrs, DUMMY, numJacobs);
183 
184  num_df_dx = numJacobs.block<7, 7>(0, 0);
185  num_df_du = numJacobs.block<7, 7>(0, 7);
186  }
187 
188  // Compare:
189  EXPECT_NEAR(0, (df_dx - num_df_dx).array().abs().sum(), 3e-3)
190  << "q1: " << q1 << endl
191  << "q2: " << q2 << endl
192  << "Numeric approximation of df_dx: " << endl
193  << num_df_dx << endl
194  << "Implemented method: " << endl
195  << df_dx << endl
196  << "Error: " << endl
197  << df_dx - num_df_dx << endl;
198 
199  EXPECT_NEAR(0, (df_du - num_df_du).array().abs().sum(), 3e-3)
200  << "q1: " << q1 << endl
201  << "q2: " << q2 << endl
202  << "Numeric approximation of df_du: " << endl
203  << num_df_du << endl
204  << "Implemented method: " << endl
205  << df_du << endl
206  << "Error: " << endl
207  << df_du - num_df_du << endl;
208  }
209 
211  double x, double y, double z, double yaw, double pitch, double roll,
212  double std_scale)
213  {
214  CPose3DQuatPDFGaussian p7pdf1 =
215  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
216 
217  CPose3DQuatPDFGaussian p7_inv = -p7pdf1;
218 
219  // Numeric approximation:
220  CVectorFixedDouble<7> y_mean;
222  {
223  CVectorFixedDouble<7> x_mean;
224  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
225 
227  x_cov.insertMatrix(0, 0, p7pdf1.cov);
228 
229  double DUMMY = 0;
230  CVectorFixedDouble<7> x_incrs;
231  x_incrs.fill(1e-6);
233  x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
234  }
235 
236  // Compare:
237  EXPECT_NEAR(0, (y_cov - p7_inv.cov).array().abs().mean(), 1e-2)
238  << "p1 mean: " << p7pdf1.mean << endl
239  << "inv mean: " << p7_inv.mean << endl
240  << "Numeric approximation of covariance: " << endl
241  << y_cov << endl
242  << "Returned covariance: " << endl
243  << p7_inv.cov << endl
244  << "Error: " << endl
245  << y_cov - p7_inv.cov << endl;
246  }
247 
249  double x, double y, double z, double yaw, double pitch, double roll,
250  double std_scale, double x2, double y2, double z2, double yaw2,
251  double pitch2, double roll2, double std_scale2)
252  {
253  CPose3DQuatPDFGaussian p7pdf1 =
254  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
255  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
256  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
257 
258  CPose3DQuatPDFGaussian p7_comp = p7pdf1 - p7pdf2;
259 
260  // Numeric approximation:
261  CVectorFixedDouble<7> y_mean;
263  {
265  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
266  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
267 
269  x_cov.insertMatrix(0, 0, p7pdf1.cov);
270  x_cov.insertMatrix(7, 7, p7pdf2.cov);
271 
272  double DUMMY = 0;
274  x_incrs.fill(1e-6);
276  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
277  }
278  // Compare:
279  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
280  << "p1 mean: " << p7pdf1.mean << endl
281  << "p2 mean: " << p7pdf2.mean << endl
282  << "Numeric approximation of covariance: " << endl
283  << y_cov << endl
284  << "Returned covariance: " << endl
285  << p7_comp.cov << endl;
286  }
287 
289  double x, double y, double z, double yaw, double pitch, double roll,
290  double std_scale, double x2, double y2, double z2, double yaw2,
291  double pitch2, double roll2)
292  {
293  CPose3DQuatPDFGaussian p7pdf1 =
294  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
295 
296  const CPose3DQuat new_base =
297  CPose3DQuat(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
298  const CPose3DQuatPDFGaussian new_base_pdf(
299  new_base, CMatrixDouble77()); // COV = Zeros
300 
301  const CPose3DQuatPDFGaussian p7_new_base_pdf = new_base_pdf + p7pdf1;
302  p7pdf1.changeCoordinatesReference(new_base);
303 
304  // Compare:
305  EXPECT_NEAR(
306  0, (p7_new_base_pdf.cov - p7pdf1.cov).array().abs().mean(), 1e-2)
307  << "p1 mean: " << p7pdf1.mean << endl
308  << "new_base: " << new_base << endl;
309  EXPECT_NEAR(
310  0,
311  (p7_new_base_pdf.mean.asVectorVal() - p7pdf1.mean.asVectorVal())
312  .array()
313  .abs()
314  .mean(),
315  1e-2)
316  << "p1 mean: " << p7pdf1.mean << endl
317  << "new_base: " << new_base << endl;
318  }
319 };
320 
321 TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
322 {
323  test_toFromYPRGauss(-30.0_deg, 10.0_deg, 60.0_deg);
324  test_toFromYPRGauss(30.0_deg, 88.0_deg, 0.0_deg);
325  test_toFromYPRGauss(30.0_deg, 89.5_deg, 0.0_deg);
326  // The formulas break at pitch=90, but this we cannot avoid...
327 }
328 
329 TEST_F(Pose3DQuatPDFGaussTests, CompositionJacobian)
330 {
331  testCompositionJacobian(
332  0, 0, 0, 2.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
333  testCompositionJacobian(
334  1, 2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
335  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, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
344  30.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 }
361 
363 {
364  testInverse(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
365  testInverse(0, 0, 0, 10.0_deg, 0.0_deg, 0.0_deg, 0.1);
366  testInverse(0, 0, 0, 0.0_deg, 10.0_deg, 0.0_deg, 0.1);
367  testInverse(0, 0, 0, 0.0_deg, 0.0_deg, 10.0_deg, 0.1);
368 
369  testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
370  testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.2);
371 
372  testInverse(1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
373  testInverse(-1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
374  testInverse(1, 2, -3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
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, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
381  testInverse(-1, 2, 3, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
382  testInverse(1, 2, -3, 0.0_deg, 30.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, 0.0_deg, 30.0_deg, 0.1);
389  testInverse(-1, 2, 3, 0.0_deg, 0.0_deg, 30.0_deg, 0.1);
390  testInverse(1, 2, -3, 0.0_deg, 0.0_deg, 30.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 }
397 
399 {
400  testPoseComposition(
401  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
402  0.0_deg, 0.1);
403  testPoseComposition(
404  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
405  0.0_deg, 0.1);
406 
407  testPoseComposition(
408  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
409  -10.0_deg, 30.0_deg, 0.1);
410  testPoseComposition(
411  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
412  -10.0_deg, 30.0_deg, 0.2);
413 
414  testPoseComposition(
415  1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
416  0.0_deg, 0.1);
417  testPoseComposition(
418  1, 2, 3, 0.0_deg, 10.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, 0.0_deg, 10.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, 0.0_deg, 0.1, -8, 45, 10, 10.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, 0.0_deg, 10.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, 0.0_deg,
431  10.0_deg, 0.1);
432 }
433 
434 TEST_F(Pose3DQuatPDFGaussTests, InverseComposition)
435 {
436  testPoseInverseComposition(
437  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
438  0.0_deg, 0.1);
439  testPoseInverseComposition(
440  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
441  0.0_deg, 0.1);
442 
443  testPoseInverseComposition(
444  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
445  -10.0_deg, 30.0_deg, 0.1);
446  testPoseInverseComposition(
447  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
448  -10.0_deg, 30.0_deg, 0.2);
449 
450  testPoseInverseComposition(
451  1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
452  0.0_deg, 0.1);
453  testPoseInverseComposition(
454  1, 2, 3, 0.0_deg, 10.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, 0.0_deg, 10.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, 0.0_deg, 0.1, -8, 45, 10, 10.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, 0.0_deg, 10.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, 0.0_deg,
467  10.0_deg, 0.1);
468 }
469 
471 {
472  testChangeCoordsRef(
473  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
474  0.0_deg);
475  testChangeCoordsRef(
476  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
477  0.0_deg);
478 
479  testChangeCoordsRef(
480  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
481  -10.0_deg, 30.0_deg);
482  testChangeCoordsRef(
483  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
484  -10.0_deg, 30.0_deg);
485 }
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
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:540
STL namespace.
CMatrixFixed< double, 7, 7 > CMatrixDouble77
Definition: CMatrixFixed.h:370
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
static void func_compose(const CVectorFixedDouble< 2 *7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &Y)
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
static void func_inv_compose(const CVectorFixedDouble< 2 *7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &Y)
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
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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 ...
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)
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.
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)
static void func_inverse(const CVectorFixedDouble< 7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &Y)
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++)
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020