10 #include <gtest/gtest.h>
16 #include <Eigen/Dense>
25 static const std::vector<mrpt::poses::CPose3D>
ptc = {
26 {.0, .0, .0, .0_deg, .0_deg, .0_deg},
27 {1.0, 2.0, 3.0, .0_deg, .0_deg, .0_deg},
28 {1.0, 2.0, 3.0, 10.0_deg, .0_deg, .0_deg},
29 {1.0, 2.0, 3.0, .0_deg, 1.0_deg, .0_deg},
30 {1.0, 2.0, 3.0, .0_deg, .0_deg, 1.0_deg},
31 {-6.0, 2.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
32 {1.0, 8.0, 0.0, -45.0_deg, 10.0_deg, 70.0_deg},
33 {1.0, 2.0, 3.0, 89.0_deg, .5_deg, 1.0_deg},
34 {1.0, -2.0, 0.4, -89.0_deg, .5_deg, 1.0_deg},
35 {1.0, 2.0, 3.0, .1_deg, 89.0_deg, 1.0_deg},
36 {1.0, -2.0, 0.4, .1_deg, -88.0_deg, 1.0_deg},
37 {1.0, 2.0, 3.0, .1_deg, .9_deg, 178.0_deg},
38 {1.0, -2.0, 0.4, .1_deg, .9_deg, -179.1_deg},
41 template <
class POSE_TYPE>
51 typename SE_TYPE::type P1, D,
P2;
54 template <
unsigned int MAT_LEN>
64 typename SE_TYPE::tangent_vector eps1, eps2;
65 for (
size_t i = 0; i < SE_TYPE::DOFs; i++)
68 eps2[i] = x[SE_TYPE::DOFs + i];
71 const POSE_TYPE incr1 = SE_TYPE::exp(eps1);
72 const POSE_TYPE incr2 = SE_TYPE::exp(eps2);
74 const POSE_TYPE P1 =
params.P1 + incr1;
75 const POSE_TYPE P2 =
params.P2 + incr2;
76 const POSE_TYPE& Pd =
params.D;
79 P1.getInverseHomogeneousMatrix(P1_inv_hm);
80 P2.getHomogeneousMatrix(P2hm);
81 Pd.getInverseHomogeneousMatrix(Pd_inv_hm);
85 const POSE_TYPE DinvP1invP2(DinvP1invP2_);
88 Y = SE_TYPE::log(DinvP1invP2);
94 const POSE_TYPE P1(P1_);
95 const POSE_TYPE Pd(Pd_);
96 const POSE_TYPE P2(P2_);
97 const POSE_TYPE Pdinv = -Pd;
99 static const int DIMS = SE_TYPE::DOFs;
103 SE_TYPE::jacob_dDinvP1invP2_de1e2(Pdinv, P1, P2, J1, J2);
109 for (
int i = 0; i < DIMS + DIMS; i++) x_mean[i] = 0;
125 &func_numeric_DinvP1InvP2),
126 x_incrs,
params, numJacobs);
128 num_J1 = numJacobs.
asEigen().block<DIMS, DIMS>(0, 0);
129 num_J2 = numJacobs.
asEigen().block<DIMS, DIMS>(0, DIMS);
132 const double max_eror = 1e-3;
135 << std::setprecision(3) <<
"p1: " << P1 << endl
136 <<
"d: " << Pd << endl
137 <<
"p2: " << P2 << endl
140 <<
"Implemented J1:\n"
143 << J1 - num_J1 << endl;
146 <<
"p1: " << P1 << endl
147 <<
"d: " << Pd << endl
148 <<
"p2: " << P2 << endl
151 <<
"Implemented J2:\n"
154 << J2 - num_J2 << endl;
162 const POSE_TYPE
A = SE_TYPE::fromManifoldVector(
params.Avec + x);
163 const POSE_TYPE B = SE_TYPE::fromManifoldVector(
params.Bvec);
164 Y = SE_TYPE::asManifoldVector(
A + B);
172 const POSE_TYPE
A = SE_TYPE::fromManifoldVector(
params.Avec);
173 const POSE_TYPE B = SE_TYPE::fromManifoldVector(
params.Bvec + x);
174 Y = SE_TYPE::asManifoldVector(
A + B);
179 const POSE_TYPE
A(A_);
180 const POSE_TYPE B(B_);
182 constexpr
auto N = SE_TYPE::MANIFOLD_DIM;
185 const auto dAB_A = SE_TYPE::jacob_dAB_dA(
A, B);
186 const auto dAB_B = SE_TYPE::jacob_dAB_dB(
A, B);
195 params.Avec = SE_TYPE::asManifoldVector(
A);
196 params.Bvec = SE_TYPE::asManifoldVector(B);
206 x_incrs,
params, numJacobs);
208 num_dAB_A = numJacobs;
218 params.Avec = SE_TYPE::asManifoldVector(
A);
219 params.Bvec = SE_TYPE::asManifoldVector(B);
229 x_incrs,
params, numJacobs);
231 num_dAB_B = numJacobs;
234 const double max_eror = 1e-3;
236 EXPECT_NEAR(0, (num_dAB_A - dAB_A).sum_abs(), max_eror)
237 << std::setprecision(3) <<
"A: " <<
A << endl
238 <<
"B: " << B << endl
239 <<
"Numeric dAB_A:\n"
241 <<
"Implemented dAB_A:\n"
244 << dAB_A - num_dAB_A << endl;
246 EXPECT_NEAR(0, (num_dAB_B - dAB_B).sum_abs(), max_eror)
247 << std::setprecision(3) <<
"A: " <<
A << endl
248 <<
"B: " << B << endl
249 <<
"Numeric dAB_B:\n"
251 <<
"Implemented dAB_B:\n"
254 << dAB_B - num_dAB_B << endl;
259 for (
const auto& p1 :
ptc)
260 for (
const auto& p2 :
ptc)
261 for (
const auto& pd :
ptc) test_jacobs_DinvP1InvP2(p1, pd, p2);
266 for (
const auto& p1 :
ptc)
267 for (
const auto& p2 :
ptc) test_jacobs_AB(p1, p2);