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>
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, const double& dummy,
75  CVectorFixedDouble<7>& Y)
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,
89  CVectorFixedDouble<7>& Y)
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 
101  void testPoseComposition(
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;
115  CMatrixFixed<double, 7, 7> y_cov;
116  {
117  CVectorFixedDouble<2 * 7> x_mean;
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 
121  CMatrixFixed<double, 14, 14> x_cov;
122  x_cov.insertMatrix(0, 0, p7pdf1.cov);
123  x_cov.insertMatrix(7, 7, p7pdf2.cov);
124 
125  double DUMMY = 0;
126  CVectorFixedDouble<2 * 7> x_incrs;
127  x_incrs.fill(1e-6);
128  transform_gaussian_linear(
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,
143  CVectorFixedDouble<7>& Y)
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 
152  void testCompositionJacobian(
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:
161  CMatrixDouble77 df_dx(UNINITIALIZED_MATRIX),
162  df_du(UNINITIALIZED_MATRIX);
163  CPose3DQuatPDF::jacobiansPoseComposition(
164  q1, // x
165  q2, // u
166  df_dx, df_du);
167 
168  // Numerical approximation:
169  CMatrixDouble77 num_df_dx(UNINITIALIZED_MATRIX),
170  num_df_du(UNINITIALIZED_MATRIX);
171  {
172  CVectorFixedDouble<2 * 7> x_mean;
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;
177  CVectorFixedDouble<2 * 7> x_incrs;
178  x_incrs.fill(1e-7);
179  CMatrixDouble numJacobs;
180  mrpt::math::estimateJacobian(
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 
213  void testInverse(
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;
224  CMatrixFixed<double, 7, 7> y_cov;
225  {
226  CVectorFixedDouble<7> x_mean;
227  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
228 
229  CMatrixFixed<double, 7, 7> x_cov;
230  x_cov.insertMatrix(0, 0, p7pdf1.cov);
231 
232  double DUMMY = 0;
233  CVectorFixedDouble<7> x_incrs;
234  x_incrs.fill(1e-6);
235  transform_gaussian_linear(
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 
251  void testPoseInverseComposition(
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;
265  CMatrixFixed<double, 7, 7> y_cov;
266  {
267  CVectorFixedDouble<2 * 7> x_mean;
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 
271  CMatrixFixed<double, 14, 14> x_cov;
272  x_cov.insertMatrix(0, 0, p7pdf1.cov);
273  x_cov.insertMatrix(7, 7, p7pdf2.cov);
274 
275  double DUMMY = 0;
276  CVectorFixedDouble<2 * 7> x_incrs;
277  x_incrs.fill(1e-6);
278  transform_gaussian_linear(
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 
291  void testChangeCoordsRef(
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(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
327  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(88), DEG2RAD(0));
328  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(89.5), DEG2RAD(0));
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, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0),
336  DEG2RAD(0), DEG2RAD(0));
337  testCompositionJacobian(
338  1, 2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
339  DEG2RAD(0), DEG2RAD(0));
340  testCompositionJacobian(
341  1, -2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
342  DEG2RAD(0), DEG2RAD(0));
343  testCompositionJacobian(
344  1, 2, -3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
345  DEG2RAD(0), DEG2RAD(0));
346  testCompositionJacobian(
347  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
348  DEG2RAD(-10), DEG2RAD(30));
349  testCompositionJacobian(
350  1, 2, 3, DEG2RAD(20), DEG2RAD(-80), DEG2RAD(70), -8, 45, 10,
351  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
352  testCompositionJacobian(
353  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(-70), -8, 45, 10,
354  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
355  testCompositionJacobian(
356  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10,
357  DEG2RAD(-50), DEG2RAD(-10), DEG2RAD(30));
358  testCompositionJacobian(
359  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
360  DEG2RAD(10), DEG2RAD(30));
361  testCompositionJacobian(
362  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
363  DEG2RAD(-10), DEG2RAD(-30));
364 }
365 
366 TEST_F(Pose3DQuatPDFGaussTests, Inverse)
367 {
368  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
369  testInverse(0, 0, 0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
370  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
371  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
372 
373  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
374  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.2);
375 
376  testInverse(1, 2, 3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
377  testInverse(-1, 2, 3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
378  testInverse(1, 2, -3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
379  testInverse(-1, 2, -3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
380  testInverse(1, 2, 3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
381  testInverse(-1, 2, 3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
382  testInverse(1, 2, -3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
383  testInverse(-1, 2, -3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
384  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
385  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
386  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
387  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
388  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
389  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
390  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
391  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(-30), DEG2RAD(0), 0.1);
392  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
393  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
394  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
395  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
396  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
397  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
398  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
399  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
400 }
401 
402 TEST_F(Pose3DQuatPDFGaussTests, Composition)
403 {
404  testPoseComposition(
405  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
406  DEG2RAD(0), DEG2RAD(0), 0.1);
407  testPoseComposition(
408  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
409  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
410 
411  testPoseComposition(
412  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
413  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
414  testPoseComposition(
415  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
416  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
417 
418  testPoseComposition(
419  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
420  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
421  testPoseComposition(
422  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
423  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
424  testPoseComposition(
425  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
426  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
427  testPoseComposition(
428  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
429  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
430  testPoseComposition(
431  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
432  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
433  testPoseComposition(
434  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
435  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
436 }
437 
438 TEST_F(Pose3DQuatPDFGaussTests, InverseComposition)
439 {
440  testPoseInverseComposition(
441  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
442  DEG2RAD(0), DEG2RAD(0), 0.1);
443  testPoseInverseComposition(
444  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
445  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
446 
447  testPoseInverseComposition(
448  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
449  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
450  testPoseInverseComposition(
451  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
452  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
453 
454  testPoseInverseComposition(
455  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
456  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
457  testPoseInverseComposition(
458  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
459  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
460  testPoseInverseComposition(
461  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
462  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
463  testPoseInverseComposition(
464  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
465  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
466  testPoseInverseComposition(
467  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
468  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
469  testPoseInverseComposition(
470  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
471  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
472 }
473 
474 TEST_F(Pose3DQuatPDFGaussTests, ChangeCoordsRef)
475 {
476  testChangeCoordsRef(
477  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
478  DEG2RAD(0), DEG2RAD(0));
479  testChangeCoordsRef(
480  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
481  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
482 
483  testChangeCoordsRef(
484  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
485  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
486  testChangeCoordsRef(
487  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
488  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
489 }
mrpt::poses::CPose3DQuatPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
Definition: CPose3DQuatPDFGaussian.cpp:207
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
z
GLdouble GLdouble z
Definition: glext.h:3879
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:324
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:43
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:204
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:505
std
STL namespace.
CPose3D.h
mrpt::math::CMatrixDouble77
CMatrixFixed< double, 7, 7 > CMatrixDouble77
Definition: CMatrixFixed.h:355
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:261
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
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:148
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::func_inv_compose
static void func_inv_compose(const CVectorFixedDouble< 2 *7 > &x, const double &dummy, CVectorFixedDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:87
Pose3DQuatPDFGaussTests::testInverse
void testInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:213
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.
Definition: CKalmanFilterCapable.h:27
transform_gaussian.h
Pose3DQuatPDFGaussTests::func_inverse
static void func_inverse(const CVectorFixedDouble< 7 > &x, const double &dummy, CVectorFixedDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:141
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
Pose3DQuatPDFGaussTests::func_compose
static void func_compose(const CVectorFixedDouble< 2 *7 > &x, const double &dummy, CVectorFixedDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:73
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:291
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:39
y
GLenum GLint GLint y
Definition: glext.h:3542
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:152
x
GLenum GLint x
Definition: glext.h:3542
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:251
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
p
GLfloat GLfloat p
Definition: glext.h:6398
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:101
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
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019