10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
15 #include <Eigen/Dense>
22 template class mrpt::CTraitsTest<CPosePDFGaussian>;
30 double x,
double y,
double phi,
double std_scale)
37 for (
int i = 0; i < 3; i++)
cov(i, i) += 1e-7;
46 const CPose2D p1(x[0], x[1], x[2]);
48 for (
int i = 0; i < 3; i++) Y[i] = p1_inv[i];
63 for (
int i = 0; i < 3; i++) x_mean[i] = pdf1.
mean[i];
71 x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
75 <<
"pdf1 mean: " << pdf1.
mean << endl
76 <<
"Numeric approximation of covariance: " << endl
78 <<
"Returned covariance: " << endl
79 << pdf1_inv.
cov << endl;
85 testPoseInverse(0, 0, 0, 0.01);
86 testPoseInverse(0, 0, 0, 0.1);
88 testPoseInverse(1, 0, 0, 0.1);
89 testPoseInverse(0, 1, 0, 0.1);
90 testPoseInverse(0, 0, 1, 0.1);
92 testPoseInverse(-5, 0, 0, 0.1);
93 testPoseInverse(0, -5, 0, 0.1);
94 testPoseInverse(0, 0, -5, 0.1);
96 testPoseInverse(4, 6, 10.0_deg, 0.1);
97 testPoseInverse(4, 6, -10.0_deg, 0.1);
99 testPoseInverse(-7, 2, 30.0_deg, 0.1);
100 testPoseInverse(-7, 2, -30.0_deg, 0.1);