10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
18 #include <Eigen/Dense>
25 template class mrpt::CTraitsTest<CPose3DQuatPDFGaussian>;
33 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
37 generateRandomPose3DPDF(x, y, z, yaw,
pitch,
roll, std_scale));
41 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
49 for (
int i = 0; i < 6; i++)
cov(i, i) += 1e-7;
58 generateRandomPose3DPDF(1.0, 2.0, 3.0, yaw,
pitch,
roll, 0.1);
67 <<
"p1quat : " << endl
80 x[7 + 0], x[7 + 1], x[7 + 2],
83 for (
int i = 0; i < 7; i++) Y[i] = p[i];
94 x[7 + 0], x[7 + 1], x[7 + 2],
97 for (
int i = 0; i < 7; i++) Y[i] = p[i];
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)
106 generateRandomPoseQuat3DPDF(x, y, z, yaw,
pitch,
roll, std_scale);
108 x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
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];
122 x_cov.insertMatrix(7, 7, p7pdf2.cov);
128 x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
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
136 <<
"Returned covariance: " << endl
137 << p7_comp.
cov << endl;
148 for (
int i = 0; i < 7; i++) Y[i] = p1_inv[i];
152 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
153 double x2,
double y2,
double z2,
double yaw2,
double pitch2,
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];
184 x_incrs, DUMMY, numJacobs);
186 num_df_dx = numJacobs.
block<7, 7>(0, 0);
187 num_df_du = numJacobs.
block<7, 7>(0, 7);
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
196 <<
"Implemented method: " << endl
199 << df_dx - num_df_dx << endl;
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
206 <<
"Implemented method: " << endl
209 << df_du - num_df_du << endl;
213 double x,
double y,
double z,
double yaw,
double pitch,
double roll,
217 generateRandomPoseQuat3DPDF(x, y, z, yaw,
pitch,
roll, std_scale);
226 for (
int i = 0; i < 7; i++) x_mean[i] = p7pdf1.
mean[i];
235 x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
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
244 <<
"Returned covariance: " << endl
245 << p7_inv.
cov << endl
247 << y_cov - p7_inv.
cov << endl;
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)
256 generateRandomPoseQuat3DPDF(x, y, z, yaw,
pitch,
roll, std_scale);
258 x2, y2, z2, yaw2, pitch2, roll2, std_scale2);
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];
272 x_cov.insertMatrix(7, 7, p7pdf2.cov);
278 x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
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
286 <<
"Returned covariance: " << endl
287 << p7_comp.
cov << endl;
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)
296 generateRandomPoseQuat3DPDF(x, y, z, yaw,
pitch,
roll, std_scale);
308 0, (p7_new_base_pdf.
cov - p7pdf1.cov).array().abs().maxCoeff(),
310 <<
"p1 mean: " << p7pdf1.mean << endl
311 <<
"new_base: " << new_base << endl;
319 <<
"p1 mean: " << p7pdf1.mean << endl
320 <<
"new_base: " << new_base << endl;
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);
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,
339 testCompositionJacobian(
340 1, -2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 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,
345 testCompositionJacobian(
346 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.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,
351 testCompositionJacobian(
352 1, 2, 3, 20.0_deg, 80.0_deg, -70.0_deg, -8, 45, 10, 50.0_deg, -10.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,
357 testCompositionJacobian(
358 1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, 10.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,
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);
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);
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);
404 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
407 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
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);
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);
418 1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
421 1, 2, 3, 0.0_deg, 10.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
424 1, 2, 3, 0.0_deg, 0.0_deg, 10.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
427 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 10.0_deg, 0.0_deg,
430 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 10.0_deg,
433 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
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,
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,
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);
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,
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,
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,
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,
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,
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,
476 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
479 1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
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);
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);