10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
17 #include <Eigen/Dense>
24 template class mrpt::CTraitsTest<CPose3D>;
37 auto I4 = CMatrixDouble44::Identity();
40 (HM.asEigen() * HMi.asEigen() - I4.asEigen()).array().abs().sum(),
44 << HMi <<
"inv(HM)*HM:\n"
45 << (HM * HMi) << endl;
50 const auto HMi_from_p1_inv =
62 <<
"p1: " << p1 <<
"p1_inv_inv: " << p1_inv_inv << endl;
64 EXPECT_NEAR((HMi_from_p1_inv - HMi).sum_abs(), 0, 1e-4)
65 <<
"HMi_from_p1_inv:\n"
66 << HMi_from_p1_inv <<
"HMi:\n"
72 const CPose3D p1_c_p2 = p1 + p2;
73 const CPose3D p1_i_p2 = p1 - p2;
75 const CPose3D p1_c_p2_i_p2 = p1_c_p2 - p1;
76 const CPose3D p2_c_p1_i_p2 = p2 + p1_i_p2;
86 <<
"p2 : " << p2 << endl
87 <<
"p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl;
97 <<
"p1 : " << p1 << endl
98 <<
"p2 : " << p2 << endl
99 <<
"p2 matrix : " << endl
101 <<
"p1_i_p2 : " << p1_i_p2 << endl
102 <<
"p1_i_p2 matrix: " << endl
104 <<
"p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl;
154 EXPECT_DOUBLE_EQ(p2d.
x(), p2d_bis.
x()) <<
"p2d: " << p2d << endl;
155 EXPECT_DOUBLE_EQ(p2d.
y(), p2d_bis.
y()) <<
"p2d: " << p2d << endl;
156 EXPECT_DOUBLE_EQ(p2d.
phi(), p2d_bis.
phi()) <<
"p2d: " << p2d << endl;
158 EXPECT_DOUBLE_EQ(p2d.
phi(), p3d.
yaw()) <<
"p2d: " << p2d << endl;
163 const CPose3D p1_plus_p2 = p1 + p2;
177 <<
"p2 : " << p2 << endl
178 <<
"p1 : " << p1 << endl
179 <<
"p1_plus_p2 : " << p1_plus_p2 << endl
180 <<
"p1_plus_p2bis : " << p1_plus_p2bis << endl;
195 <<
"p2 : " << p2 << endl
196 <<
"p1 : " << p1 << endl
197 <<
"p1_plus_p2 : " << p1_plus_p2 << endl
198 <<
"p1_plus_p2bis : " << p1_plus_p2bis << endl;
213 <<
"p2 : " << p2 << endl
214 <<
"p1 : " << p1 << endl
215 <<
"p1_plus_p2 : " << p1_plus_p2 << endl
216 <<
"p1_plus_p2bis : " << p1_plus_p2bis << endl;
227 p.x(), p.y(), p.z(), p1_plus_p2.
x(), p1_plus_p2.
y(),
248 std::abs(lx - p1_plus_p.
x()) + std::abs(ly - p1_plus_p.
y()) +
249 std::abs(lz - p1_plus_p.z()),
256 p1.inverseComposePoint(
257 p1_plus_p.
x(), p1_plus_p.
y(), p1_plus_p.z(), p_recov2.
x(),
258 p_recov2.
y(), p_recov2.z());
283 CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
284 const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
286 for (
int i = 0; i < 3; i++) Y[i] = pp[i];
293 CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
294 const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
302 const CPose3D& p1,
double x,
double y,
double z,
bool use_aprox =
false)
311 x, y, z, pp.
x, pp.
y, pp.
z, df_dpoint, df_dpose, std::nullopt,
319 for (
int i = 0; i < 6; i++) x_mean[i] = p1[i];
333 x_incrs, DUMMY, numJacobs);
335 num_df_dpose = numJacobs.
block<3, 6>(0, 0);
336 num_df_dpoint = numJacobs.
block<3, 3>(0, 6);
339 const double max_error = use_aprox ? 0.1 : 3e-3;
341 EXPECT_NEAR(0, (df_dpoint - num_df_dpoint).sum_abs(), max_error)
342 <<
"p1: " << p1 << endl
343 <<
"p: " << p << endl
344 <<
"Numeric approximation of df_dpoint: " << endl
345 << num_df_dpoint << endl
346 <<
"Implemented method: " << endl
349 << df_dpoint - num_df_dpoint << endl;
353 (df_dpose.
asEigen() - num_df_dpose.
asEigen()).array().abs().sum(),
355 <<
"p1: " << p1 << endl
356 <<
"p: " << p << endl
357 <<
"Numeric approximation of df_dpose: " << endl
358 << num_df_dpose.
asEigen() << endl
359 <<
"Implemented method: " << endl
369 <<
"p1: " << p1 << endl;
373 const CPose3D& p1,
double x,
double y,
double z)
388 for (
int i = 0; i < 6; i++) x_mean[i] = p1[i];
402 x_incrs, DUMMY, numJacobs);
404 num_df_dpose = numJacobs.
block<3, 6>(0, 0);
405 num_df_dpoint = numJacobs.
block<3, 3>(0, 6);
409 0, (df_dpoint - num_df_dpoint).asEigen().array().abs().
sum(), 3e-3)
410 <<
"p1: " << p1 << endl
411 <<
"p: " << p << endl
412 <<
"Numeric approximation of df_dpoint: " << endl
413 << num_df_dpoint << endl
414 <<
"Implemented method: " << endl
417 << df_dpoint - num_df_dpoint << endl;
421 (df_dpose.
asEigen() - num_df_dpose.
asEigen()).array().abs().sum(),
423 <<
"p1: " << p1 << endl
424 <<
"p: " << p << endl
425 <<
"Numeric approximation of df_dpose: " << endl
426 << num_df_dpose.
asEigen() << endl
427 <<
"Implemented method: " << endl
441 for (
size_t i = 0; i < 4; i++)
442 for (
size_t j = 0; j < 4; j++)
445 i == j ? 1.0 : 0.0, 1e-8)
446 <<
"Failed for (i,j)=" << i <<
"," << j << endl
447 <<
"Matrix is: " << endl
449 <<
"case was: " << label << endl;
459 for (
int i = 0; i < 3; i++) Y[i] = pp[i];
469 for (
int i = 0; i < 3; i++) Y[i] = pp[i];
478 x_l.
x, x_l.
y, x_l.
z, pp.
x, pp.
y, pp.
z, std::nullopt, std::nullopt,
485 for (
int i = 0; i < 6; i++) x_mean[i] = 0;
488 for (
int i = 0; i < 3; i++) P[i] = pp[i];
497 &func_compose_point_se3),
498 x_incrs, P, num_df_dse3);
502 0, (df_dse3.
asEigen() - num_df_dse3.
asEigen()).array().abs().sum(),
504 <<
"p: " << p << endl
505 <<
"x_l: " << x_l << endl
506 <<
"Numeric approximation of df_dse3: " << endl
507 << num_df_dse3.
asEigen() << endl
508 <<
"Implemented method: " << endl
520 x_g.
x, x_g.
y, x_g.
z, pp.
x, pp.
y, pp.
z, std::nullopt, std::nullopt,
527 for (
int i = 0; i < 6; i++) x_mean[i] = 0;
530 for (
int i = 0; i < 3; i++) P[i] = pp[i];
539 &func_invcompose_point_se3),
540 x_incrs, P, num_df_dse3);
544 0, (df_dse3.
asEigen() - num_df_dse3.
asEigen()).array().abs().sum(),
546 <<
"p: " << p << endl
547 <<
"x_g: " << x_g << endl
548 <<
"Numeric approximation of df_dse3: " << endl
549 << num_df_dse3.
asEigen() << endl
550 <<
"Implemented method: " << endl
570 for (
int i = 0; i < 6; i++) x_mean[i] = 0;
572 const double& dummy = 0.;
581 x_incrs, dummy, numJacobs);
588 double vals[12 * 6] = {
589 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0,
591 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
593 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0,
595 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0};
602 << M.
asEigen() <<
"numJacobs:\n"
603 << numJacobs <<
"\n";
616 R.asEigen().jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
617 R = Rsvd.matrixU() * Rsvd.matrixV().transpose();
633 const double& dummy = 0.;
641 x_incrs, dummy, numJacobs);
645 (numJacobs.
asEigen() - theor_jacob.asEigen()).array().abs().sum(),
647 <<
"Pose: " << p << endl
652 << theor_jacob.
asEigen() << endl
654 << theor_jacob.asEigen() - numJacobs.
asEigen() << endl;
662 const CPose3D expe_D = incr + D;
684 x_incrs, p, numJacobs);
688 (numJacobs.
asEigen() - theor_jacob.asEigen())
693 <<
"Pose: " << p << endl
698 << theor_jacob.
asEigen() << endl
700 << theor_jacob.asEigen() - numJacobs.
asEigen() << endl;
708 const CPose3D expe_D = D + incr;
730 x_incrs, p, numJacobs);
734 (numJacobs.
asEigen() - theor_jacob.asEigen())
739 <<
"Pose: " << p << endl
744 << theor_jacob.
asEigen() << endl
746 << theor_jacob.asEigen() - numJacobs.
asEigen() << endl;
786 x_incrs,
params, numJacobs);
790 (numJacobs.
asEigen() - theor_jacob.asEigen())
795 <<
"Pose A: " <<
A << endl
796 <<
"Pose D: " << D << endl
800 << theor_jacob.
asEigen() << endl
802 << theor_jacob.asEigen() - numJacobs.
asEigen() << endl;
811 test_default_values(p,
"Default");
816 test_default_values(p,
"p=p2");
820 test_default_values(p1 + p2,
"p1+p2");
822 test_default_values(p,
"p=p1+p2");
827 test_default_values(p,
"p1-p2");
833 CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
844 CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
855 static const std::vector<mrpt::poses::CPose3D>
ptc = {
856 {.0, .0, .0, .0_deg, .0_deg, .0_deg},
857 {1.0, 2.0, 3.0, .0_deg, .0_deg, .0_deg},
858 {1.0, 2.0, 3.0, 10.0_deg, .0_deg, .0_deg},
859 {1.0, 2.0, 3.0, .0_deg, 1.0_deg, .0_deg},
860 {1.0, 2.0, 3.0, .0_deg, .0_deg, 1.0_deg},
861 {1.0, 2.0, 3.0, 80.0_deg, 5.0_deg, 5.0_deg},
862 {1.0, 2.0, 3.0, -20.0_deg, -30.0_deg, -40.0_deg},
863 {1.0, 2.0, 3.0, -45.0_deg, 10.0_deg, 70.0_deg},
864 {1.0, 2.0, 3.0, 40.0_deg, -5.0_deg, 25.0_deg},
865 {1.0, 2.0, 3.0, 40.0_deg, 20.0_deg, -15.0_deg},
866 {-6.0, 2.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
867 {6.0, -5.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
868 {6.0, 2.0, -9.0, 40.0_deg, 20.0_deg, 15.0_deg},
869 {0.0, 8.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
870 {1.0, 0.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
871 {1.0, 8.0, 0.0, -45.0_deg, 10.0_deg, 70.0_deg}};
876 for (
const auto& p :
ptc) test_inverse(p);
881 for (
const auto& p1 :
ptc)
882 for (
const auto& p2 :
ptc) test_compose(p1, p2);
886 for (
const auto& p1 :
ptc)
887 for (
const auto& p2 :
ptc) test_composeFrom(p1, p2);
892 for (
const auto& p :
ptc) test_to_from_2d(p.x(), p.y(), p.yaw());
897 for (
const auto& p :
ptc)
899 test_composePoint(p, 10, 11, 12);
900 test_composePoint(p, -5, 1, 2);
901 test_composePoint(p, 5, -1, 2);
902 test_composePoint(p, 5, 1, -2);
908 for (
const auto& p :
ptc)
910 test_composePointJacob(p, 10, 11, 12);
911 test_composePointJacob(p, -5, 1, 2);
917 test_composePointJacob(
920 test_composePointJacob(
923 test_composePointJacob(
926 test_composePointJacob(
933 for (
const auto& p :
ptc)
935 test_invComposePointJacob(p, 10, 11, 12);
936 test_invComposePointJacob(p, -5, 1, 2);
937 test_invComposePointJacob(p, 5, -1, 2);
938 test_invComposePointJacob(p, 5, 1, -2);
944 for (
const auto& p :
ptc)
946 test_composePointJacob_se3(p,
TPoint3D(0, 0, 0));
947 test_composePointJacob_se3(p,
TPoint3D(10, 11, 12));
948 test_composePointJacob_se3(p,
TPoint3D(-5.0, -15.0, 8.0));
953 for (
const auto& p :
ptc)
955 test_invComposePointJacob_se3(p,
TPoint3D(0, 0, 0));
956 test_invComposePointJacob_se3(p,
TPoint3D(10, 11, 12));
957 test_invComposePointJacob_se3(p,
TPoint3D(-5.0, -15.0, 8.0));
963 for (
const auto& p :
ptc) test_ExpLnEqual(p);
981 for (
const auto& p :
ptc) test_Jacob_dexpeD_de(p);
986 for (
const auto& p :
ptc) test_Jacob_dDexpe_de(p);
991 for (
const auto& p1 :
ptc)
992 for (
const auto& p2 :
ptc) test_Jacob_dAexpeD_de(p1, p2);