10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
17 #include <Eigen/Dense>
24 template class mrpt::CTraitsTest<CPose3DPDFGaussian>;
32 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
40 for (
int i = 0; i < 6; i++)
cov(i, i) += 1e-7;
46 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
50 generateRandomPose3DPDF(x, y, z, yaw,
pitch,
roll, std_scale);
57 const double val_mean_error =
60 const double cov_mean_error = (p6pdf_recov.
cov - p6pdf.
cov).sum_abs();
71 const CPose3D p1(x[0], x[1], x[2], x[3], x[4], x[5]);
73 x[6 + 0], x[6 + 1], x[6 + 2], x[6 + 3], x[6 + 4], x[6 + 5]);
75 for (
int i = 0; i < 6; i++) Y[i] = p[i];
82 const CPose3D p1(x[0], x[1], x[2], x[3], x[4], x[5]);
84 x[6 + 0], x[6 + 1], x[6 + 2], x[6 + 3], x[6 + 4], x[6 + 5]);
86 for (
int i = 0; i < 6; i++) Y[i] = p[i];
91 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
92 double std_scale,
double x2,
double y2,
double z2,
double yaw2,
93 double pitch2,
double roll2,
double std_scale2)
96 generateRandomPose3DPDF(x, y, z, yaw,
pitch,
roll, std_scale);
98 x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
108 for (
int i = 0; i < 6; i++) x_mean[i] = p6pdf1.
mean[i];
109 for (
int i = 0; i < 6; i++) x_mean[6 + i] = p6pdf2.mean[i];
113 x_cov.insertMatrix(6, 6, p6pdf2.cov);
119 x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
123 <<
"p1 mean: " << p6pdf1.
mean << endl
124 <<
"p2 mean: " << p6pdf2.mean << endl;
128 <<
"p1 mean: " << p6pdf1.
mean << endl
129 <<
"p2 mean: " << p6pdf2.mean << endl;
137 <<
"p1 mean: " << p6pdf1.
mean << endl
138 <<
"p2 mean: " << p6pdf2.mean << endl;
142 <<
"p1 mean: " << p6pdf1.
mean << endl
143 <<
"p2 mean: " << p6pdf2.mean << endl;
147 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
148 double x2,
double y2,
double z2,
double yaw2,
double pitch2,
152 const CPose3D q2(x2, y2, z2, yaw2, pitch2, roll2);
167 for (
int i = 0; i < 6; i++) x_mean[i] = q1[i];
168 for (
int i = 0; i < 6; i++) x_mean[6 + i] = q2[i];
179 x_incrs, DUMMY, numJacobs);
181 num_df_dx = numJacobs.
block<6, 6>(0, 0);
182 num_df_du = numJacobs.
block<6, 6>(0, 6);
186 EXPECT_NEAR(0, (df_dx - num_df_dx).sum_abs(), 3e-3)
187 <<
"q1: " << q1 << endl
188 <<
"q2: " << q2 << endl
189 <<
"Numeric approximation of df_dx: " << endl
191 <<
"Implemented method: " << endl
194 << df_dx - num_df_dx << endl;
196 EXPECT_NEAR(0, (df_du - num_df_du).sum_abs(), 3e-3)
197 <<
"q1: " << q1 << endl
198 <<
"q2: " << q2 << endl
199 <<
"Numeric approximation of df_du: " << endl
201 <<
"Implemented method: " << endl
204 << df_du - num_df_du << endl;
209 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
210 double std_scale,
double x2,
double y2,
double z2,
double yaw2,
211 double pitch2,
double roll2,
double std_scale2)
214 generateRandomPose3DPDF(x, y, z, yaw,
pitch,
roll, std_scale);
216 x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
226 for (
int i = 0; i < 6; i++) x_mean[i] = p6pdf1.
mean[i];
227 for (
int i = 0; i < 6; i++) x_mean[6 + i] = p6pdf2.mean[i];
231 x_cov.insertMatrix(6, 6, p6pdf2.cov);
237 x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
241 <<
"p1 mean: " << p6pdf1.
mean << endl
242 <<
"p2 mean: " << p6pdf2.mean << endl;
246 <<
"p1 mean: " << p6pdf1.
mean << endl
247 <<
"p2 mean: " << p6pdf2.mean << endl;
255 <<
"p1 mean: " << p6pdf1.
mean << endl
256 <<
"p2 mean: " << p6pdf2.mean << endl;
260 <<
"p1 mean: " << p6pdf1.
mean << endl
261 <<
"p2 mean: " << p6pdf2.mean << endl;
266 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
270 generateRandomPose3DPDF(x, y, z, yaw,
pitch,
roll, std_scale);
288 <<
"p mean: " << p6pdf2.mean << endl;
292 <<
"p mean: " << p6pdf2.mean << endl;
306 <<
"p mean: " << p6pdf2.mean << endl
307 <<
"p6_inv2 mean: " << p6_inv2.
mean << endl
308 <<
"p6_comp mean: " << p6_comp.
mean << endl;
312 <<
"p mean: " << p6pdf2.mean << endl
313 <<
"p6_inv2 mean: " << p6_inv2.
mean << endl
314 <<
"p6_comp mean: " << p6_comp.
mean << endl;
319 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
320 double std_scale,
double x2,
double y2,
double z2,
double yaw2,
321 double pitch2,
double roll2,
double std_scale2)
325 x, y, z, yaw,
pitch,
roll, std_scale, x2, y2, z2, yaw2, pitch2,
328 testPoseInverseComposition(
329 x, y, z, yaw,
pitch,
roll, std_scale, x2, y2, z2, yaw2, pitch2,
332 testPoseInverse(x, y, z, yaw,
pitch,
roll, std_scale);
336 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
337 double std_scale,
double x2,
double y2,
double z2,
double yaw2,
338 double pitch2,
double roll2)
341 generateRandomPose3DPDF(x, y, z, yaw,
pitch,
roll, std_scale);
343 const CPose3D new_base =
CPose3D(x2, y2, z2, yaw2, pitch2, roll2);
352 0, (p6_new_base_pdf.
cov - p6pdf1.cov).array().abs().mean(), 1e-2)
353 <<
"p1 mean: " << p6pdf1.mean << endl
354 <<
"new_base: " << new_base << endl;
362 <<
"p1 mean: " << p6pdf1.mean << endl
363 <<
"new_base: " << new_base << endl;
369 testToQuatPDFAndBack(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
370 testToQuatPDFAndBack(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.2);
372 testToQuatPDFAndBack(6, -2, -3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
373 testToQuatPDFAndBack(6, -2, -3, 0.0_deg, 0.0_deg, 0.0_deg, 0.2);
375 testToQuatPDFAndBack(6, -2, -3, 10.0_deg, 40.0_deg, 5.0_deg, 0.1);
376 testToQuatPDFAndBack(6, -2, -3, 10.0_deg, 40.0_deg, 5.0_deg, 0.2);
378 testToQuatPDFAndBack(6, -2, -3, -50.0_deg, 87.0_deg, 20.0_deg, 0.1);
379 testToQuatPDFAndBack(6, -2, -3, -50.0_deg, 87.0_deg, 20.0_deg, 0.2);
381 testToQuatPDFAndBack(6, -2, -3, -50.0_deg, -87.0_deg, 20.0_deg, 0.1);
382 testToQuatPDFAndBack(6, -2, -3, -50.0_deg, -87.0_deg, 20.0_deg, 0.2);
387 testCompositionJacobian(
388 0, 0, 0, 2.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
389 testCompositionJacobian(
390 0, 0, 0, 2.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
391 testCompositionJacobian(
392 1, 2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
394 testCompositionJacobian(
395 1, -2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
397 testCompositionJacobian(
398 1, 2, -3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
400 testCompositionJacobian(
401 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
403 testCompositionJacobian(
404 1, 2, 3, 20.0_deg, -80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
406 testCompositionJacobian(
407 1, 2, 3, 20.0_deg, 80.0_deg, -70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
409 testCompositionJacobian(
410 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, -50.0_deg, -10.0_deg,
412 testCompositionJacobian(
413 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, 10.0_deg,
415 testCompositionJacobian(
416 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
423 testAllPoseOperators(
424 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
426 testAllPoseOperators(
427 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
430 testAllPoseOperators(
431 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
432 -10.0_deg, 30.0_deg, 0.1);
433 testAllPoseOperators(
434 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
435 -10.0_deg, 30.0_deg, 0.2);
437 testAllPoseOperators(
438 1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
440 testAllPoseOperators(
441 1, 2, 3, 0.0_deg, 10.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
443 testAllPoseOperators(
444 1, 2, 3, 0.0_deg, 0.0_deg, 10.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
446 testAllPoseOperators(
447 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 10.0_deg, 0.0_deg,
449 testAllPoseOperators(
450 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 10.0_deg,
452 testAllPoseOperators(
453 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
460 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
463 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
467 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
468 -10.0_deg, 30.0_deg);
470 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
471 -10.0_deg, 30.0_deg);