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-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>
13 #include <mrpt/math/transform_gaussian.h>
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/CPose3DPDFGaussian.h>
16 #include <mrpt/poses/CPose3DQuatPDFGaussian.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 {}
32  static CPose3DQuatPDFGaussian generateRandomPoseQuat3DPDF(
33  double x, double y, double z, double yaw, double pitch, double roll,
34  double std_scale)
35  {
36  return CPose3DQuatPDFGaussian(
37  generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale));
38  }
39 
40  static CPose3DPDFGaussian generateRandomPose3DPDF(
41  double x, double y, double z, double yaw, double pitch, double roll,
42  double std_scale)
43  {
44  CMatrixDouble61 r;
45  mrpt::random::getRandomGenerator().drawGaussian1DMatrix(
46  r, 0, std_scale);
47  CMatrixDouble66 cov;
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);
59  CPose3DQuatPDFGaussian p1quat = CPose3DQuatPDFGaussian(p1ypr);
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,
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(
87  const CVectorFixedDouble<2 * 7>& x,
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 
99  void testPoseComposition(
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;
113  CMatrixFixed<double, 7, 7> y_cov;
114  {
115  CVectorFixedDouble<2 * 7> x_mean;
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 
119  CMatrixFixed<double, 14, 14> x_cov;
120  x_cov.insertMatrix(0, 0, p7pdf1.cov);
121  x_cov.insertMatrix(7, 7, p7pdf2.cov);
122 
123  double DUMMY = 0;
124  CVectorFixedDouble<2 * 7> x_incrs;
125  x_incrs.fill(1e-6);
126  transform_gaussian_linear(
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,
141  CVectorFixedDouble<7>& Y)
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 
149  void testCompositionJacobian(
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:
158  CMatrixDouble77 df_dx(UNINITIALIZED_MATRIX),
159  df_du(UNINITIALIZED_MATRIX);
160  CPose3DQuatPDF::jacobiansPoseComposition(
161  q1, // x
162  q2, // u
163  df_dx, df_du);
164 
165  // Numerical approximation:
166  CMatrixDouble77 num_df_dx(UNINITIALIZED_MATRIX),
167  num_df_du(UNINITIALIZED_MATRIX);
168  {
169  CVectorFixedDouble<2 * 7> x_mean;
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;
174  CVectorFixedDouble<2 * 7> x_incrs;
175  x_incrs.fill(1e-7);
176  CMatrixDouble numJacobs;
177  mrpt::math::estimateJacobian(
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 
210  void testInverse(
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;
221  CMatrixFixed<double, 7, 7> y_cov;
222  {
223  CVectorFixedDouble<7> x_mean;
224  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
225 
226  CMatrixFixed<double, 7, 7> x_cov;
227  x_cov.insertMatrix(0, 0, p7pdf1.cov);
228 
229  double DUMMY = 0;
230  CVectorFixedDouble<7> x_incrs;
231  x_incrs.fill(1e-6);
232  transform_gaussian_linear(
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 
248  void testPoseInverseComposition(
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;
262  CMatrixFixed<double, 7, 7> y_cov;
263  {
264  CVectorFixedDouble<2 * 7> x_mean;
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 
268  CMatrixFixed<double, 14, 14> x_cov;
269  x_cov.insertMatrix(0, 0, p7pdf1.cov);
270  x_cov.insertMatrix(7, 7, p7pdf2.cov);
271 
272  double DUMMY = 0;
273  CVectorFixedDouble<2 * 7> x_incrs;
274  x_incrs.fill(1e-6);
275  transform_gaussian_linear(
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 
288  void testChangeCoordsRef(
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 
362 TEST_F(Pose3DQuatPDFGaussTests, Inverse)
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 
398 TEST_F(Pose3DQuatPDFGaussTests, Composition)
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 
470 TEST_F(Pose3DQuatPDFGaussTests, ChangeCoordsRef)
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 }
mrpt::poses::CPose3DQuatPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
Definition: CPose3DQuatPDFGaussian.cpp:205
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
Pose3DQuatPDFGaussTests::generateRandomPose3DPDF
static CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:40
Pose3DQuatPDFGaussTests::test_toFromYPRGauss
void test_toFromYPRGauss(double yaw, double pitch, double roll)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:54
TEST_F
TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:321
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
mrpt::poses::CPose3DQuatPDFGaussian::cov
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
Definition: CPose3DQuatPDFGaussian.h:81
mrpt::math::CQuaternionDouble
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:540
std
STL namespace.
CPose3D.h
mrpt::math::CMatrixDouble77
CMatrixFixed< double, 7, 7 > CMatrixDouble77
Definition: CMatrixFixed.h:370
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
Pose3DQuatPDFGaussTests::func_compose
static void func_compose(const CVectorFixedDouble< 2 *7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:73
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
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
Pose3DQuatPDFGaussTests::func_inv_compose
static void func_inv_compose(const CVectorFixedDouble< 2 *7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:86
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:221
mrpt::poses::CPose3DQuat
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
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
Pose3DQuatPDFGaussTests::testInverse
void testInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:210
CPose3DPDFGaussian.h
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:82
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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
num_jacobian.h
Pose3DQuatPDFGaussTests::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: CPose3DQuatPDFGaussian_unittest.cpp:288
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:39
Pose3DQuatPDFGaussTests::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: CPose3DQuatPDFGaussian_unittest.cpp:149
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
mrpt::poses::CPose3DQuatPDF::jacobiansPoseComposition
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$.
Definition: CPose3DQuatPDF.cpp:42
Pose3DQuatPDFGaussTests::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: CPose3DQuatPDFGaussian_unittest.cpp:248
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
Pose3DQuatPDFGaussTests::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: CPose3DQuatPDFGaussian_unittest.cpp:99
Pose3DQuatPDFGaussTests::func_inverse
static void func_inverse(const CVectorFixedDouble< 7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:139
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
Pose3DQuatPDFGaussTests::generateRandomPoseQuat3DPDF
static CPose3DQuatPDFGaussian generateRandomPoseQuat3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:32


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