MRPT  2.0.4
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().maxCoeff(), 1e-6)
65  << "p1ypr: " << endl
66  << p1ypr << endl
67  << "p1quat : " << endl
68  << p1quat << endl
69  << "p2ypr : " << endl
70  << p2ypr << endl;
71  }
72 
73  static void func_compose(
74  const CVectorFixedDouble<2 * 7>& x,
75  [[maybe_unused]] const double& dummy, CVectorFixedDouble<7>& Y)
76  {
77  const CPose3DQuat p1(
78  x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
79  const CPose3DQuat p2(
80  x[7 + 0], x[7 + 1], x[7 + 2],
81  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
82  const CPose3DQuat p = p1 + p2;
83  for (int i = 0; i < 7; i++) Y[i] = p[i];
84  }
85 
86  static void func_inv_compose(
87  const CVectorFixedDouble<2 * 7>& x,
88  [[maybe_unused]] const double& dummy, CVectorFixedDouble<7>& Y)
89  {
90  CQuaternionDouble q(x[3], x[4], x[5], x[6]);
91  q.normalize();
92  const CPose3DQuat p1(x[0], x[1], x[2], q);
93  const CPose3DQuat p2(
94  x[7 + 0], x[7 + 1], x[7 + 2],
95  CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
96  const CPose3DQuat p = p1 - p2;
97  for (int i = 0; i < 7; i++) Y[i] = p[i];
98  }
99 
100  void testPoseComposition(
101  double x, double y, double z, double yaw, double pitch, double roll,
102  double std_scale, double x2, double y2, double z2, double yaw2,
103  double pitch2, double roll2, double std_scale2)
104  {
105  CPose3DQuatPDFGaussian p7pdf1 =
106  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
107  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
108  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
109 
110  CPose3DQuatPDFGaussian p7_comp = p7pdf1 + p7pdf2;
111 
112  // Numeric approximation:
113  CVectorFixedDouble<7> y_mean;
114  CMatrixFixed<double, 7, 7> y_cov;
115  {
116  CVectorFixedDouble<2 * 7> x_mean;
117  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
118  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
119 
120  CMatrixFixed<double, 14, 14> x_cov;
121  x_cov.insertMatrix(0, 0, p7pdf1.cov);
122  x_cov.insertMatrix(7, 7, p7pdf2.cov);
123 
124  double DUMMY = 0;
125  CVectorFixedDouble<2 * 7> x_incrs;
126  x_incrs.fill(1e-6);
127  transform_gaussian_linear(
128  x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
129  }
130  // Compare:
131  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().maxCoeff(), 1e-3)
132  << "p1 mean: " << p7pdf1.mean << endl
133  << "p2 mean: " << p7pdf2.mean << endl
134  << "Numeric approximation of covariance: " << endl
135  << y_cov << endl
136  << "Returned covariance: " << endl
137  << p7_comp.cov << endl;
138  }
139 
140  static void func_inverse(
141  const CVectorFixedDouble<7>& x, [[maybe_unused]] const double& dummy,
142  CVectorFixedDouble<7>& Y)
143  {
144  CQuaternionDouble q(x[3], x[4], x[5], x[6]);
145  q.normalize();
146  const CPose3DQuat p1(x[0], x[1], x[2], q);
147  const CPose3DQuat p1_inv(-p1);
148  for (int i = 0; i < 7; i++) Y[i] = p1_inv[i];
149  }
150 
151  void testCompositionJacobian(
152  double x, double y, double z, double yaw, double pitch, double roll,
153  double x2, double y2, double z2, double yaw2, double pitch2,
154  double roll2)
155  {
156  const CPose3DQuat q1(CPose3D(x, y, z, yaw, pitch, roll));
157  const CPose3DQuat q2(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
158 
159  // Theoretical Jacobians:
160  CMatrixDouble77 df_dx(UNINITIALIZED_MATRIX),
161  df_du(UNINITIALIZED_MATRIX);
162  CPose3DQuatPDF::jacobiansPoseComposition(
163  q1, // x
164  q2, // u
165  df_dx, df_du);
166 
167  // Numerical approximation:
168  CMatrixDouble77 num_df_dx(UNINITIALIZED_MATRIX),
169  num_df_du(UNINITIALIZED_MATRIX);
170  {
171  CVectorFixedDouble<2 * 7> x_mean;
172  for (int i = 0; i < 7; i++) x_mean[i] = q1[i];
173  for (int i = 0; i < 7; i++) x_mean[7 + i] = q2[i];
174 
175  double DUMMY = 0;
176  CVectorFixedDouble<2 * 7> x_incrs;
177  x_incrs.fill(1e-7);
178  CMatrixDouble numJacobs;
179  mrpt::math::estimateJacobian(
180  x_mean,
181  std::function<void(
182  const CVectorFixedDouble<2 * 7>& x, const double& dummy,
183  CVectorFixedDouble<7>& Y)>(&func_compose),
184  x_incrs, DUMMY, numJacobs);
185 
186  num_df_dx = numJacobs.block<7, 7>(0, 0);
187  num_df_du = numJacobs.block<7, 7>(0, 7);
188  }
189 
190  // Compare:
191  EXPECT_NEAR(0, (df_dx - num_df_dx).array().abs().maxCoeff(), 1e-6)
192  << "q1: " << q1 << endl
193  << "q2: " << q2 << endl
194  << "Numeric approximation of df_dx: " << endl
195  << num_df_dx << endl
196  << "Implemented method: " << endl
197  << df_dx << endl
198  << "Error: " << endl
199  << df_dx - num_df_dx << endl;
200 
201  EXPECT_NEAR(0, (df_du - num_df_du).array().abs().maxCoeff(), 1e-6)
202  << "q1: " << q1 << endl
203  << "q2: " << q2 << endl
204  << "Numeric approximation of df_du: " << endl
205  << num_df_du << endl
206  << "Implemented method: " << endl
207  << df_du << endl
208  << "Error: " << endl
209  << df_du - num_df_du << endl;
210  }
211 
212  void testInverse(
213  double x, double y, double z, double yaw, double pitch, double roll,
214  double std_scale)
215  {
216  CPose3DQuatPDFGaussian p7pdf1 =
217  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
218 
219  CPose3DQuatPDFGaussian p7_inv = -p7pdf1;
220 
221  // Numeric approximation:
222  CVectorFixedDouble<7> y_mean;
223  CMatrixFixed<double, 7, 7> y_cov;
224  {
225  CVectorFixedDouble<7> x_mean;
226  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
227 
228  CMatrixFixed<double, 7, 7> x_cov;
229  x_cov.insertMatrix(0, 0, p7pdf1.cov);
230 
231  double DUMMY = 0;
232  CVectorFixedDouble<7> x_incrs;
233  x_incrs.fill(1e-6);
234  transform_gaussian_linear(
235  x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
236  }
237 
238  // Compare:
239  EXPECT_NEAR(0, (y_cov - p7_inv.cov).array().abs().maxCoeff(), 1e-6)
240  << "p1 mean: " << p7pdf1.mean << endl
241  << "inv mean: " << p7_inv.mean << endl
242  << "Numeric approximation of covariance: " << endl
243  << y_cov << endl
244  << "Returned covariance: " << endl
245  << p7_inv.cov << endl
246  << "Error: " << endl
247  << y_cov - p7_inv.cov << endl;
248  }
249 
250  void testPoseInverseComposition(
251  double x, double y, double z, double yaw, double pitch, double roll,
252  double std_scale, double x2, double y2, double z2, double yaw2,
253  double pitch2, double roll2, double std_scale2)
254  {
255  CPose3DQuatPDFGaussian p7pdf1 =
256  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
257  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
258  x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
259 
260  CPose3DQuatPDFGaussian p7_comp = p7pdf1 - p7pdf2;
261 
262  // Numeric approximation:
263  CVectorFixedDouble<7> y_mean;
264  CMatrixFixed<double, 7, 7> y_cov;
265  {
266  CVectorFixedDouble<2 * 7> x_mean;
267  for (int i = 0; i < 7; i++) x_mean[i] = p7pdf1.mean[i];
268  for (int i = 0; i < 7; i++) x_mean[7 + i] = p7pdf2.mean[i];
269 
270  CMatrixFixed<double, 14, 14> x_cov;
271  x_cov.insertMatrix(0, 0, p7pdf1.cov);
272  x_cov.insertMatrix(7, 7, p7pdf2.cov);
273 
274  double DUMMY = 0;
275  CVectorFixedDouble<2 * 7> x_incrs;
276  x_incrs.fill(1e-6);
277  transform_gaussian_linear(
278  x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
279  }
280  // Compare:
281  EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().maxCoeff(), 1e-6)
282  << "p1 mean: " << p7pdf1.mean << endl
283  << "p2 mean: " << p7pdf2.mean << endl
284  << "Numeric approximation of covariance: " << endl
285  << y_cov << endl
286  << "Returned covariance: " << endl
287  << p7_comp.cov << endl;
288  }
289 
290  void testChangeCoordsRef(
291  double x, double y, double z, double yaw, double pitch, double roll,
292  double std_scale, double x2, double y2, double z2, double yaw2,
293  double pitch2, double roll2)
294  {
295  CPose3DQuatPDFGaussian p7pdf1 =
296  generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
297 
298  const CPose3DQuat new_base =
299  CPose3DQuat(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
300  const CPose3DQuatPDFGaussian new_base_pdf(
301  new_base, CMatrixDouble77()); // COV = Zeros
302 
303  const CPose3DQuatPDFGaussian p7_new_base_pdf = new_base_pdf + p7pdf1;
304  p7pdf1.changeCoordinatesReference(new_base);
305 
306  // Compare:
307  EXPECT_NEAR(
308  0, (p7_new_base_pdf.cov - p7pdf1.cov).array().abs().maxCoeff(),
309  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:205
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::math::CQuaternion::normalize
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:301
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
Pose3DQuatPDFGaussTests::func_compose
static void func_compose(const CVectorFixedDouble< 2 *7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:73
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::MatrixVectorBase::block
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
Definition: MatrixVectorBase.h:138
Pose3DQuatPDFGaussTests::func_inv_compose
static void func_inv_compose(const CVectorFixedDouble< 2 *7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:86
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
mrpt::math::transform_gaussian_linear
void transform_gaussian_linear(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const VECTORLIKE1 &x_increments)
First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for a...
Definition: transform_gaussian.h:152
mrpt::math::cov
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:149
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::math::estimateJacobian
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:31
Pose3DQuatPDFGaussTests::testInverse
void testInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:212
CPose3DPDFGaussian.h
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:82
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
transform_gaussian.h
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
num_jacobian.h
Pose3DQuatPDFGaussTests::testChangeCoordsRef
void testChangeCoordsRef(double x, double y, double z, double yaw, double pitch, double roll, double std_scale, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:290
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:151
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
mrpt::math::CQuaternion
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
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:250
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:100
Pose3DQuatPDFGaussTests::func_inverse
static void func_inverse(const CVectorFixedDouble< 7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:140
mrpt::poses::CPoseOrPoint::asVectorVal
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:266
CPose3DQuatPDFGaussian.h
for
for(unsigned int i=0;i< NUM_IMGS;i++)
Definition: chessboard_stereo_camera_calib_unittest.cpp:33
Pose3DQuatPDFGaussTests::generateRandomPoseQuat3DPDF
static CPose3DQuatPDFGaussian generateRandomPoseQuat3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:32


Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020