Main MRPT website > C++ reference for MRPT 1.9.9
CPose3DQuatPDFGaussian_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <mrpt/poses/CPose3DPDFGaussian.h>
11 #include <mrpt/poses/CPose3DQuatPDFGaussian.h>
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/random.h>
14 #include <mrpt/math/transform_gaussian.h>
15 #include <mrpt/math/jacobians.h>
16 #include <mrpt/utils/CTraitsTest.h>
17 #include <gtest/gtest.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 using namespace mrpt::utils;
22 using namespace mrpt::math;
23 using namespace std;
24 
25 template class mrpt::utils::CTraitsTest<CPose3DQuatPDFGaussian>;
26 
27 class Pose3DQuatPDFGaussTests : public ::testing::Test
28 {
29  protected:
30  virtual void SetUp() {}
31  virtual void TearDown() {}
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(r, 0, std_scale);
46  CMatrixDouble66 cov;
47  cov.multiply_AAt(r); // random semi-definite positive matrix:
48  for (int i = 0; i < 6; i++) cov(i, i) += 1e-7;
49  CPose3DPDFGaussian p6pdf(CPose3D(x, y, z, yaw, pitch, roll), cov);
50  return p6pdf;
51  }
52 
53  void test_toFromYPRGauss(double yaw, double pitch, double roll)
54  {
55  // Random pose:
56  CPose3DPDFGaussian p1ypr =
57  generateRandomPose3DPDF(1.0, 2.0, 3.0, yaw, pitch, roll, 0.1);
58  CPose3DQuatPDFGaussian p1quat = CPose3DQuatPDFGaussian(p1ypr);
59 
60  // Convert back to a 6x6 representation:
61  CPose3DPDFGaussian p2ypr = CPose3DPDFGaussian(p1quat);
62 
63  EXPECT_NEAR(0, (p2ypr.cov - p1ypr.cov).array().abs().mean(), 1e-2)
64  << "p1ypr: " << endl
65  << p1ypr << endl
66  << "p1quat : " << endl
67  << p1quat << endl
68  << "p2ypr : " << endl
69  << p2ypr << endl;
70  }
71 
72  static void func_compose(
73  const CArrayDouble<2 * 7>& x, const double& dummy, CArrayDouble<7>& Y)
74  {
75  MRPT_UNUSED_PARAM(dummy);
76  const CPose3DQuat p1(
77  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
78  const CPose3DQuat p2(
79  x[7 + 0], x[7 + 1], x[7 + 2],
80  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
81  const CPose3DQuat p = p1 + p2;
82  for (int i = 0; i < 7; i++) Y[i] = p[i];
83  }
84 
85  static void func_inv_compose(
86  const CArrayDouble<2 * 7>& x, const double& dummy, CArrayDouble<7>& Y)
87  {
88  MRPT_UNUSED_PARAM(dummy);
89  const CPose3DQuat p1(
90  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
91  const CPose3DQuat p2(
92  x[7 + 0], x[7 + 1], x[7 + 2],
93  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
94  const CPose3DQuat p = p1 - p2;
95  for (int i = 0; i < 7; i++) Y[i] = p[i];
96  }
97 
98  void testPoseComposition(
99  double x, double y, double z, double yaw, double pitch, double roll,
100  double std_scale, double x2, double y2, double z2, double yaw2,
101  double pitch2, double roll2, double std_scale2)
102  {
103  CPose3DQuatPDFGaussian p7pdf1 =
104  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
105  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
106  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
107 
108  CPose3DQuatPDFGaussian p7_comp = p7pdf1 + p7pdf2;
109 
110  // Numeric approximation:
111  CArrayDouble<7> y_mean;
112  CMatrixFixedNumeric<double, 7, 7> y_cov;
113  {
114  CArrayDouble<2 * 7> x_mean;
115  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
116  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
117 
118  CMatrixFixedNumeric<double, 14, 14> x_cov;
119  x_cov.insertMatrix(0, 0, p7pdf1.cov);
120  x_cov.insertMatrix(7, 7, p7pdf2.cov);
121 
122  double DUMMY = 0;
123  CArrayDouble<2 * 7> x_incrs;
124  x_incrs.assign(1e-6);
125  transform_gaussian_linear(
126  x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
127  }
128  // Compare:
129  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
130  << "p1 mean: " << p7pdf1.mean << endl
131  << "p2 mean: " << p7pdf2.mean << endl
132  << "Numeric approximation of covariance: " << endl
133  << y_cov << endl
134  << "Returned covariance: " << endl
135  << p7_comp.cov << endl;
136  }
137 
138  static void func_inverse(
139  const CArrayDouble<7>& x, const double& dummy, CArrayDouble<7>& Y)
140  {
141  MRPT_UNUSED_PARAM(dummy);
142  const CPose3DQuat p1(
143  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
144  const CPose3DQuat p1_inv(-p1);
145  for (int i = 0; i < 7; i++) Y[i] = p1_inv[i];
146  }
147 
148  void testCompositionJacobian(
149  double x, double y, double z, double yaw, double pitch, double roll,
150  double x2, double y2, double z2, double yaw2, double pitch2,
151  double roll2)
152  {
153  const CPose3DQuat q1(CPose3D(x, y, z, yaw, pitch, roll));
154  const CPose3DQuat q2(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
155 
156  // Theoretical Jacobians:
157  CMatrixDouble77 df_dx(UNINITIALIZED_MATRIX),
158  df_du(UNINITIALIZED_MATRIX);
159  CPose3DQuatPDF::jacobiansPoseComposition(
160  q1, // x
161  q2, // u
162  df_dx, df_du);
163 
164  // Numerical approximation:
165  CMatrixDouble77 num_df_dx(UNINITIALIZED_MATRIX),
166  num_df_du(UNINITIALIZED_MATRIX);
167  {
168  CArrayDouble<2 * 7> x_mean;
169  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
170  for (int i = 0; i < 7; i++) x_mean[7 + i] = q2[i];
171 
172  double DUMMY = 0;
173  CArrayDouble<2 * 7> x_incrs;
174  x_incrs.assign(1e-7);
175  CMatrixDouble numJacobs;
176  mrpt::math::jacobians::jacob_numeric_estimate(
177  x_mean, std::function<void(
178  const CArrayDouble<2 * 7>& x, const double& dummy,
179  CArrayDouble<7>& Y)>(&func_compose),
180  x_incrs, DUMMY, numJacobs);
181 
182  numJacobs.extractMatrix(0, 0, num_df_dx);
183  numJacobs.extractMatrix(0, 7, num_df_du);
184  }
185 
186  // Compare:
187  EXPECT_NEAR(0, (df_dx - num_df_dx).array().abs().sum(), 3e-3)
188  << "q1: " << q1 << endl
189  << "q2: " << q2 << endl
190  << "Numeric approximation of df_dx: " << endl
191  << num_df_dx << endl
192  << "Implemented method: " << endl
193  << df_dx << endl
194  << "Error: " << endl
195  << df_dx - num_df_dx << endl;
196 
197  EXPECT_NEAR(0, (df_du - num_df_du).array().abs().sum(), 3e-3)
198  << "q1: " << q1 << endl
199  << "q2: " << q2 << endl
200  << "Numeric approximation of df_du: " << endl
201  << num_df_du << endl
202  << "Implemented method: " << endl
203  << df_du << endl
204  << "Error: " << endl
205  << df_du - num_df_du << endl;
206  }
207 
208  void testInverse(
209  double x, double y, double z, double yaw, double pitch, double roll,
210  double std_scale)
211  {
212  CPose3DQuatPDFGaussian p7pdf1 =
213  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
214 
215  CPose3DQuatPDFGaussian p7_inv = -p7pdf1;
216 
217  // Numeric approximation:
218  CArrayDouble<7> y_mean;
219  CMatrixFixedNumeric<double, 7, 7> y_cov;
220  {
221  CArrayDouble<7> x_mean;
222  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
223 
224  CMatrixFixedNumeric<double, 7, 7> x_cov;
225  x_cov.insertMatrix(0, 0, p7pdf1.cov);
226 
227  double DUMMY = 0;
228  CArrayDouble<7> x_incrs;
229  x_incrs.assign(1e-6);
230  transform_gaussian_linear(
231  x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
232  }
233 
234  // Compare:
235  EXPECT_NEAR(0, (y_cov - p7_inv.cov).array().abs().mean(), 1e-2)
236  << "p1 mean: " << p7pdf1.mean << endl
237  << "inv mean: " << p7_inv.mean << endl
238  << "Numeric approximation of covariance: " << endl
239  << y_cov << endl
240  << "Returned covariance: " << endl
241  << p7_inv.cov << endl
242  << "Error: " << endl
243  << y_cov - p7_inv.cov << endl;
244  }
245 
246  void testPoseInverseComposition(
247  double x, double y, double z, double yaw, double pitch, double roll,
248  double std_scale, double x2, double y2, double z2, double yaw2,
249  double pitch2, double roll2, double std_scale2)
250  {
251  CPose3DQuatPDFGaussian p7pdf1 =
252  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
253  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
254  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
255 
256  CPose3DQuatPDFGaussian p7_comp = p7pdf1 - p7pdf2;
257 
258  // Numeric approximation:
259  CArrayDouble<7> y_mean;
260  CMatrixFixedNumeric<double, 7, 7> y_cov;
261  {
262  CArrayDouble<2 * 7> x_mean;
263  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
264  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
265 
266  CMatrixFixedNumeric<double, 14, 14> x_cov;
267  x_cov.insertMatrix(0, 0, p7pdf1.cov);
268  x_cov.insertMatrix(7, 7, p7pdf2.cov);
269 
270  double DUMMY = 0;
271  CArrayDouble<2 * 7> x_incrs;
272  x_incrs.assign(1e-6);
273  transform_gaussian_linear(
274  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
275  }
276  // Compare:
277  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
278  << "p1 mean: " << p7pdf1.mean << endl
279  << "p2 mean: " << p7pdf2.mean << endl
280  << "Numeric approximation of covariance: " << endl
281  << y_cov << endl
282  << "Returned covariance: " << endl
283  << p7_comp.cov << endl;
284  }
285 
286  void testChangeCoordsRef(
287  double x, double y, double z, double yaw, double pitch, double roll,
288  double std_scale, double x2, double y2, double z2, double yaw2,
289  double pitch2, double roll2)
290  {
291  CPose3DQuatPDFGaussian p7pdf1 =
292  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
293 
294  const CPose3DQuat new_base =
295  CPose3DQuat(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
296  const CPose3DQuatPDFGaussian new_base_pdf(
297  new_base, CMatrixDouble77()); // COV = Zeros
298 
299  const CPose3DQuatPDFGaussian p7_new_base_pdf = new_base_pdf + p7pdf1;
300  p7pdf1.changeCoordinatesReference(new_base);
301 
302  // Compare:
303  EXPECT_NEAR(
304  0, (p7_new_base_pdf.cov - p7pdf1.cov).array().abs().mean(), 1e-2)
305  << "p1 mean: " << p7pdf1.mean << endl
306  << "new_base: " << new_base << endl;
307  EXPECT_NEAR(
308  0, (p7_new_base_pdf.mean.getAsVectorVal() -
309  p7pdf1.mean.getAsVectorVal())
310  .array()
311  .abs()
312  .mean(),
313  1e-2)
314  << "p1 mean: " << p7pdf1.mean << endl
315  << "new_base: " << new_base << endl;
316  }
317 };
318 
319 TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
320 {
321  test_toFromYPRGauss(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
322  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(88), DEG2RAD(0));
323  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(89.5), DEG2RAD(0));
324  // The formulas break at pitch=90, but this we cannot avoid...
325 }
326 
327 TEST_F(Pose3DQuatPDFGaussTests, CompositionJacobian)
328 {
329  testCompositionJacobian(
330  0, 0, 0, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0),
331  DEG2RAD(0), DEG2RAD(0));
332  testCompositionJacobian(
333  1, 2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
334  DEG2RAD(0), DEG2RAD(0));
335  testCompositionJacobian(
336  1, -2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
337  DEG2RAD(0), DEG2RAD(0));
338  testCompositionJacobian(
339  1, 2, -3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
340  DEG2RAD(0), DEG2RAD(0));
341  testCompositionJacobian(
342  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
343  DEG2RAD(-10), DEG2RAD(30));
344  testCompositionJacobian(
345  1, 2, 3, DEG2RAD(20), DEG2RAD(-80), DEG2RAD(70), -8, 45, 10,
346  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
347  testCompositionJacobian(
348  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(-70), -8, 45, 10,
349  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
350  testCompositionJacobian(
351  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10,
352  DEG2RAD(-50), DEG2RAD(-10), DEG2RAD(30));
353  testCompositionJacobian(
354  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
355  DEG2RAD(10), DEG2RAD(30));
356  testCompositionJacobian(
357  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
358  DEG2RAD(-10), DEG2RAD(-30));
359 }
360 
361 TEST_F(Pose3DQuatPDFGaussTests, Inverse)
362 {
363  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
364  testInverse(0, 0, 0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
365  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
366  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
367 
368  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
369  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.2);
370 
371  testInverse(1, 2, 3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
372  testInverse(-1, 2, 3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
373  testInverse(1, 2, -3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
374  testInverse(-1, 2, -3, DEG2RAD(30), DEG2RAD(0), DEG2RAD(0), 0.1);
375  testInverse(1, 2, 3, DEG2RAD(-30), DEG2RAD(0), DEG2RAD(0), 0.1);
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(0), DEG2RAD(30), DEG2RAD(0), 0.1);
380  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
381  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
382  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(30), DEG2RAD(0), 0.1);
383  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(-30), 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(0), DEG2RAD(30), 0.1);
388  testInverse(-1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
389  testInverse(1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
390  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(30), 0.1);
391  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 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 }
396 
397 TEST_F(Pose3DQuatPDFGaussTests, Composition)
398 {
399  testPoseComposition(
400  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
401  DEG2RAD(0), DEG2RAD(0), 0.1);
402  testPoseComposition(
403  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
404  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
405 
406  testPoseComposition(
407  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
408  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
409  testPoseComposition(
410  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
411  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
412 
413  testPoseComposition(
414  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
415  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
416  testPoseComposition(
417  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
418  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
419  testPoseComposition(
420  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
421  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
422  testPoseComposition(
423  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
424  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
425  testPoseComposition(
426  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
427  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
428  testPoseComposition(
429  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
430  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
431 }
432 
433 TEST_F(Pose3DQuatPDFGaussTests, InverseComposition)
434 {
435  testPoseInverseComposition(
436  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
437  DEG2RAD(0), DEG2RAD(0), 0.1);
438  testPoseInverseComposition(
439  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
440  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
441 
442  testPoseInverseComposition(
443  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
444  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
445  testPoseInverseComposition(
446  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
447  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
448 
449  testPoseInverseComposition(
450  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
451  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
452  testPoseInverseComposition(
453  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
454  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
455  testPoseInverseComposition(
456  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
457  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
458  testPoseInverseComposition(
459  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
460  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
461  testPoseInverseComposition(
462  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
463  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
464  testPoseInverseComposition(
465  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
466  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
467 }
468 
469 TEST_F(Pose3DQuatPDFGaussTests, ChangeCoordsRef)
470 {
471  testChangeCoordsRef(
472  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
473  DEG2RAD(0), DEG2RAD(0));
474  testChangeCoordsRef(
475  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
476  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
477 
478  testChangeCoordsRef(
479  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
480  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
481  testChangeCoordsRef(
482  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
483  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
484 }
mrpt::math::CMatrixDouble77
CMatrixFixedNumeric< double, 7, 7 > CMatrixDouble77
Definition: eigen_frwds.h:58
mrpt::poses::CPose3DQuatPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
Definition: CPose3DQuatPDFGaussian.cpp:271
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
z
GLdouble GLdouble z
Definition: glext.h:3872
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
mrpt::poses::CPoseOrPoint::getAsVectorVal
mrpt::math::CVectorDouble getAsVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:265
Pose3DQuatPDFGaussTests::test_toFromYPRGauss
void test_toFromYPRGauss(double yaw, double pitch, double roll)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:53
z2
INT32 z2
Definition: jidctint.cpp:130
TEST_F
TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:319
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:167
mrpt::math::CQuaternionDouble
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:504
mrpt::poses::CPose3DQuatPDFGaussian::cov
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
Definition: CPose3DQuatPDFGaussian.h:82
for
for(ctr=DCTSIZE;ctr > 0;ctr--)
Definition: jidctflt.cpp:56
std
STL namespace.
CPose3D.h
mrpt::poses::CPose3DQuatPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
Definition: CPose3DQuatPDFGaussian.h:44
mrpt::math::jacobians::jacob_numeric_estimate
void jacob_numeric_estimate(const VECTORLIKE &x, std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::m...
Definition: jacobians.h:144
Pose3DQuatPDFGaussTests::func_compose
static void func_compose(const CArrayDouble< 2 *7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:72
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:39
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: CColouredPointsMap.h:366
CTraitsTest.h
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
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:48
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:154
DEG2RAD
#define DEG2RAD
Definition: CColouredPointsMap.h:112
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
Pose3DQuatPDFGaussTests::testInverse
void testInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:208
CPose3DPDFGaussian.h
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:83
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
transform_gaussian.h
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
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:286
mrpt::math::CArrayDouble
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:87
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:40
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::math::cov
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > 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
Pose3DQuatPDFGaussTests::func_inverse
static void func_inverse(const CArrayDouble< 7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:138
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:148
x
GLenum GLint x
Definition: glext.h:3538
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:41
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:246
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:25
Pose3DQuatPDFGaussTests::func_inv_compose
static void func_inv_compose(const CArrayDouble< 2 *7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:85
p
GLfloat GLfloat p
Definition: glext.h:6305
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:98
CPose3DQuatPDFGaussian.h
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: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019