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-2018, 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/num_jacobian.h>
16 #include <CTraitsTest.h>
17 #include <gtest/gtest.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::poses;
21 
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  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(
46  r, 0, std_scale);
47  CMatrixDouble66 cov;
48  cov.multiply_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 CArrayDouble<2 * 7>& x, const double& dummy, CArrayDouble<7>& Y)
75  {
76  MRPT_UNUSED_PARAM(dummy);
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 CArrayDouble<2 * 7>& x, const double& dummy, CArrayDouble<7>& Y)
88  {
89  MRPT_UNUSED_PARAM(dummy);
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  CArrayDouble<7> y_mean;
113  CMatrixFixedNumeric<double, 7, 7> y_cov;
114  {
115  CArrayDouble<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  CMatrixFixedNumeric<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  CArrayDouble<2 * 7> x_incrs;
125  x_incrs.assign(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 CArrayDouble<7>& x, const double& dummy, CArrayDouble<7>& Y)
141  {
142  MRPT_UNUSED_PARAM(dummy);
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  CArrayDouble<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  CArrayDouble<2 * 7> x_incrs;
175  x_incrs.assign(1e-7);
176  CMatrixDouble numJacobs;
177  mrpt::math::estimateJacobian(
178  x_mean, std::function<void(
179  const CArrayDouble<2 * 7>& x, const double& dummy,
180  CArrayDouble<7>& Y)>(&func_compose),
181  x_incrs, DUMMY, numJacobs);
182 
183  numJacobs.extractMatrix(0, 0, num_df_dx);
184  numJacobs.extractMatrix(0, 7, num_df_du);
185  }
186 
187  // Compare:
188  EXPECT_NEAR(0, (df_dx - num_df_dx).array().abs().sum(), 3e-3)
189  << "q1: " << q1 << endl
190  << "q2: " << q2 << endl
191  << "Numeric approximation of df_dx: " << endl
192  << num_df_dx << endl
193  << "Implemented method: " << endl
194  << df_dx << endl
195  << "Error: " << endl
196  << df_dx - num_df_dx << endl;
197 
198  EXPECT_NEAR(0, (df_du - num_df_du).array().abs().sum(), 3e-3)
199  << "q1: " << q1 << endl
200  << "q2: " << q2 << endl
201  << "Numeric approximation of df_du: " << endl
202  << num_df_du << endl
203  << "Implemented method: " << endl
204  << df_du << endl
205  << "Error: " << endl
206  << df_du - num_df_du << endl;
207  }
208 
209  void testInverse(
210  double x, double y, double z, double yaw, double pitch, double roll,
211  double std_scale)
212  {
213  CPose3DQuatPDFGaussian p7pdf1 =
214  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
215 
216  CPose3DQuatPDFGaussian p7_inv = -p7pdf1;
217 
218  // Numeric approximation:
219  CArrayDouble<7> y_mean;
220  CMatrixFixedNumeric<double, 7, 7> y_cov;
221  {
222  CArrayDouble<7> x_mean;
223  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
224 
225  CMatrixFixedNumeric<double, 7, 7> x_cov;
226  x_cov.insertMatrix(0, 0, p7pdf1.cov);
227 
228  double DUMMY = 0;
229  CArrayDouble<7> x_incrs;
230  x_incrs.assign(1e-6);
231  transform_gaussian_linear(
232  x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
233  }
234 
235  // Compare:
236  EXPECT_NEAR(0, (y_cov - p7_inv.cov).array().abs().mean(), 1e-2)
237  << "p1 mean: " << p7pdf1.mean << endl
238  << "inv mean: " << p7_inv.mean << endl
239  << "Numeric approximation of covariance: " << endl
240  << y_cov << endl
241  << "Returned covariance: " << endl
242  << p7_inv.cov << endl
243  << "Error: " << endl
244  << y_cov - p7_inv.cov << endl;
245  }
246 
247  void testPoseInverseComposition(
248  double x, double y, double z, double yaw, double pitch, double roll,
249  double std_scale, double x2, double y2, double z2, double yaw2,
250  double pitch2, double roll2, double std_scale2)
251  {
252  CPose3DQuatPDFGaussian p7pdf1 =
253  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
254  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
255  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
256 
257  CPose3DQuatPDFGaussian p7_comp = p7pdf1 - p7pdf2;
258 
259  // Numeric approximation:
260  CArrayDouble<7> y_mean;
261  CMatrixFixedNumeric<double, 7, 7> y_cov;
262  {
263  CArrayDouble<2 * 7> x_mean;
264  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
265  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
266 
267  CMatrixFixedNumeric<double, 14, 14> x_cov;
268  x_cov.insertMatrix(0, 0, p7pdf1.cov);
269  x_cov.insertMatrix(7, 7, p7pdf2.cov);
270 
271  double DUMMY = 0;
272  CArrayDouble<2 * 7> x_incrs;
273  x_incrs.assign(1e-6);
274  transform_gaussian_linear(
275  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
276  }
277  // Compare:
278  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().mean(), 1e-2)
279  << "p1 mean: " << p7pdf1.mean << endl
280  << "p2 mean: " << p7pdf2.mean << endl
281  << "Numeric approximation of covariance: " << endl
282  << y_cov << endl
283  << "Returned covariance: " << endl
284  << p7_comp.cov << endl;
285  }
286 
287  void testChangeCoordsRef(
288  double x, double y, double z, double yaw, double pitch, double roll,
289  double std_scale, double x2, double y2, double z2, double yaw2,
290  double pitch2, double roll2)
291  {
292  CPose3DQuatPDFGaussian p7pdf1 =
293  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
294 
295  const CPose3DQuat new_base =
296  CPose3DQuat(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
297  const CPose3DQuatPDFGaussian new_base_pdf(
298  new_base, CMatrixDouble77()); // COV = Zeros
299 
300  const CPose3DQuatPDFGaussian p7_new_base_pdf = new_base_pdf + p7pdf1;
301  p7pdf1.changeCoordinatesReference(new_base);
302 
303  // Compare:
304  EXPECT_NEAR(
305  0, (p7_new_base_pdf.cov - p7pdf1.cov).array().abs().mean(), 1e-2)
306  << "p1 mean: " << p7pdf1.mean << endl
307  << "new_base: " << new_base << endl;
308  EXPECT_NEAR(
309  0, (p7_new_base_pdf.mean.getAsVectorVal() -
310  p7pdf1.mean.getAsVectorVal())
311  .array()
312  .abs()
313  .mean(),
314  1e-2)
315  << "p1 mean: " << p7pdf1.mean << endl
316  << "new_base: " << new_base << endl;
317  }
318 };
319 
320 TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
321 {
322  test_toFromYPRGauss(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
323  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(88), DEG2RAD(0));
324  test_toFromYPRGauss(DEG2RAD(30), DEG2RAD(89.5), DEG2RAD(0));
325  // The formulas break at pitch=90, but this we cannot avoid...
326 }
327 
328 TEST_F(Pose3DQuatPDFGaussTests, CompositionJacobian)
329 {
330  testCompositionJacobian(
331  0, 0, 0, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0),
332  DEG2RAD(0), DEG2RAD(0));
333  testCompositionJacobian(
334  1, 2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
335  DEG2RAD(0), DEG2RAD(0));
336  testCompositionJacobian(
337  1, -2, 3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
338  DEG2RAD(0), DEG2RAD(0));
339  testCompositionJacobian(
340  1, 2, -3, DEG2RAD(2), DEG2RAD(0), DEG2RAD(0), -8, 45, 10, DEG2RAD(0),
341  DEG2RAD(0), DEG2RAD(0));
342  testCompositionJacobian(
343  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
344  DEG2RAD(-10), DEG2RAD(30));
345  testCompositionJacobian(
346  1, 2, 3, DEG2RAD(20), DEG2RAD(-80), DEG2RAD(70), -8, 45, 10,
347  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
348  testCompositionJacobian(
349  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(-70), -8, 45, 10,
350  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
351  testCompositionJacobian(
352  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10,
353  DEG2RAD(-50), DEG2RAD(-10), DEG2RAD(30));
354  testCompositionJacobian(
355  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
356  DEG2RAD(10), DEG2RAD(30));
357  testCompositionJacobian(
358  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), -8, 45, 10, DEG2RAD(50),
359  DEG2RAD(-10), DEG2RAD(-30));
360 }
361 
362 TEST_F(Pose3DQuatPDFGaussTests, Inverse)
363 {
364  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
365  testInverse(0, 0, 0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
366  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
367  testInverse(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
368 
369  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
370  testInverse(1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.2);
371 
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(-30), DEG2RAD(0), 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(-30), DEG2RAD(0), 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  testInverse(-1, 2, -3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(-30), 0.1);
396 }
397 
398 TEST_F(Pose3DQuatPDFGaussTests, Composition)
399 {
400  testPoseComposition(
401  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
402  DEG2RAD(0), DEG2RAD(0), 0.1);
403  testPoseComposition(
404  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
405  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
406 
407  testPoseComposition(
408  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
409  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
410  testPoseComposition(
411  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
412  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
413 
414  testPoseComposition(
415  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
416  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
417  testPoseComposition(
418  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
419  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
420  testPoseComposition(
421  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
422  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
423  testPoseComposition(
424  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
425  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
426  testPoseComposition(
427  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
428  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
429  testPoseComposition(
430  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
431  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
432 }
433 
434 TEST_F(Pose3DQuatPDFGaussTests, InverseComposition)
435 {
436  testPoseInverseComposition(
437  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
438  DEG2RAD(0), DEG2RAD(0), 0.1);
439  testPoseInverseComposition(
440  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
441  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
442 
443  testPoseInverseComposition(
444  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
445  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.1);
446  testPoseInverseComposition(
447  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
448  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30), 0.2);
449 
450  testPoseInverseComposition(
451  1, 2, 3, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
452  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
453  testPoseInverseComposition(
454  1, 2, 3, DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1, -8, 45, 10,
455  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
456  testPoseInverseComposition(
457  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1, -8, 45, 10,
458  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1);
459  testPoseInverseComposition(
460  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
461  DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0.1);
462  testPoseInverseComposition(
463  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
464  DEG2RAD(0), DEG2RAD(10), DEG2RAD(0), 0.1);
465  testPoseInverseComposition(
466  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
467  DEG2RAD(0), DEG2RAD(0), DEG2RAD(10), 0.1);
468 }
469 
470 TEST_F(Pose3DQuatPDFGaussTests, ChangeCoordsRef)
471 {
472  testChangeCoordsRef(
473  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, 0, 0, 0, DEG2RAD(0),
474  DEG2RAD(0), DEG2RAD(0));
475  testChangeCoordsRef(
476  1, 2, 3, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0.1, -8, 45, 10,
477  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
478 
479  testChangeCoordsRef(
480  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.1, -8, 45, 10,
481  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
482  testChangeCoordsRef(
483  1, 2, 3, DEG2RAD(20), DEG2RAD(80), DEG2RAD(70), 0.2, -8, 45, 10,
484  DEG2RAD(50), DEG2RAD(-10), DEG2RAD(30));
485 }
TEST_F
TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:320
Pose3DQuatPDFGaussTests::test_toFromYPRGauss
void test_toFromYPRGauss(double yaw, double pitch, double roll)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:54
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
Pose3DQuatPDFGaussTests::testInverse
void testInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:209
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:287
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::generateRandomPoseQuat3DPDF
static CPose3DQuatPDFGaussian generateRandomPoseQuat3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:32
Pose3DQuatPDFGaussTests::func_inv_compose
static void func_inv_compose(const CArrayDouble< 2 *7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:86
Pose3DQuatPDFGaussTests::func_compose
static void func_compose(const CArrayDouble< 2 *7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:73
Pose3DQuatPDFGaussTests::func_inverse
static void func_inverse(const CArrayDouble< 7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:139
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:247
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::math::CArrayNumeric
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
Definition: CArrayNumeric.h:26
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:47
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:87
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:41
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:83
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:48
mrpt::poses::CPose3DQuatPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
Definition: CPose3DQuatPDFGaussian.h:45
mrpt::poses::CPose3DQuatPDFGaussian::cov
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
Definition: CPose3DQuatPDFGaussian.h:82
mrpt::poses::CPose3DQuatPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
Definition: CPose3DQuatPDFGaussian.cpp:247
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,...
Definition: CPose3DQuatPDF.cpp:41
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:263
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:174
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
y
GLenum GLint GLint y
Definition: glext.h:3538
x
GLenum GLint x
Definition: glext.h:3538
p
GLfloat GLfloat p
Definition: glext.h:6305
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
z
GLdouble GLdouble z
Definition: glext.h:3872
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:148
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:12
mrpt::math::CQuaternionDouble
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:502
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:29
mrpt::math::UNINITIALIZED_MATRIX
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
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
mrpt::math::CMatrixDouble77
CMatrixFixedNumeric< double, 7, 7 > CMatrixDouble77
Definition: eigen_frwds.h:60
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:209
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:26
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:19
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:31
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42
num_jacobian.h
transform_gaussian.h


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 814d80880 Fri Aug 24 01:51:28 2018 +0200 at mar 26 may 2026 12:30:59 CEST