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, 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(-30.0_deg, 10.0_deg, 60.0_deg);
327  test_toFromYPRGauss(30.0_deg, 88.0_deg, 0.0_deg);
328  test_toFromYPRGauss(30.0_deg, 89.5_deg, 0.0_deg);
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, 2.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0, 0.0_deg, 0.0_deg, 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, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
344  0.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  testCompositionJacobian(
361  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
362  -30.0_deg);
363 }
364 
365 TEST_F(Pose3DQuatPDFGaussTests, Inverse)
366 {
367  testInverse(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
368  testInverse(0, 0, 0, 10.0_deg, 0.0_deg, 0.0_deg, 0.1);
369  testInverse(0, 0, 0, 0.0_deg, 10.0_deg, 0.0_deg, 0.1);
370  testInverse(0, 0, 0, 0.0_deg, 0.0_deg, 10.0_deg, 0.1);
371 
372  testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
373  testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.2);
374 
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, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
381  testInverse(1, 2, -3, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
382  testInverse(-1, 2, -3, -30.0_deg, 0.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, -30.0_deg, 0.0_deg, 0.1);
389  testInverse(1, 2, -3, 0.0_deg, -30.0_deg, 0.0_deg, 0.1);
390  testInverse(-1, 2, -3, 0.0_deg, -30.0_deg, 0.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  testInverse(-1, 2, 3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
397  testInverse(1, 2, -3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
398  testInverse(-1, 2, -3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
399 }
400 
401 TEST_F(Pose3DQuatPDFGaussTests, Composition)
402 {
403  testPoseComposition(
404  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
405  0.0_deg, 0.1);
406  testPoseComposition(
407  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
408  0.0_deg, 0.1);
409 
410  testPoseComposition(
411  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
412  -10.0_deg, 30.0_deg, 0.1);
413  testPoseComposition(
414  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
415  -10.0_deg, 30.0_deg, 0.2);
416 
417  testPoseComposition(
418  1, 2, 3, 10.0_deg, 0.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, 10.0_deg, 0.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, 10.0_deg, 0.1, -8, 45, 10, 0.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, 10.0_deg, 0.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, 10.0_deg,
431  0.0_deg, 0.1);
432  testPoseComposition(
433  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
434  10.0_deg, 0.1);
435 }
436 
437 TEST_F(Pose3DQuatPDFGaussTests, InverseComposition)
438 {
439  testPoseInverseComposition(
440  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
441  0.0_deg, 0.1);
442  testPoseInverseComposition(
443  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
444  0.0_deg, 0.1);
445 
446  testPoseInverseComposition(
447  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
448  -10.0_deg, 30.0_deg, 0.1);
449  testPoseInverseComposition(
450  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
451  -10.0_deg, 30.0_deg, 0.2);
452 
453  testPoseInverseComposition(
454  1, 2, 3, 10.0_deg, 0.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, 10.0_deg, 0.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, 10.0_deg, 0.1, -8, 45, 10, 0.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, 10.0_deg, 0.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, 10.0_deg,
467  0.0_deg, 0.1);
468  testPoseInverseComposition(
469  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
470  10.0_deg, 0.1);
471 }
472 
473 TEST_F(Pose3DQuatPDFGaussTests, ChangeCoordsRef)
474 {
475  testChangeCoordsRef(
476  0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
477  0.0_deg);
478  testChangeCoordsRef(
479  1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
480  0.0_deg);
481 
482  testChangeCoordsRef(
483  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
484  -10.0_deg, 30.0_deg);
485  testChangeCoordsRef(
486  1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
487  -10.0_deg, 30.0_deg);
488 }
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
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::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
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: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::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.
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
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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
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
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:251
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: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
for
for(unsigned int i=0;i< NUM_IMGS;i++)
Definition: chessboard_stereo_camera_calib_unittest.cpp:33
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: 3a26b90fd Wed Mar 25 20:17:03 2020 +0100 at mié mar 25 23:05:41 CET 2020