13 #include <gtest/gtest.h> 21 template <
class POSE_TYPE>
31 typename SE_TYPE::pose_t P1, D,
P2;
38 typename SE_TYPE::array_t eps1, eps2;
39 for (
int i = 0; i < SE_TYPE::VECTOR_SIZE; i++)
42 eps2[i] =
x[SE_TYPE::VECTOR_SIZE + i];
45 POSE_TYPE incr1, incr2;
46 SE_TYPE::exp(eps1, incr1);
47 SE_TYPE::exp(eps2, incr2);
49 const POSE_TYPE P1 = incr1 +
params.P1;
50 const POSE_TYPE P2 = incr2 +
params.P2;
51 const POSE_TYPE& Pd =
params.D;
55 P1.getHomogeneousMatrixVal() * Pd.getHomogeneousMatrixVal() *
56 P2.getInverseHomogeneousMatrix()));
57 const POSE_TYPE P1DP2inv(P1DP2inv_);
60 SE_TYPE::pseudo_ln(P1DP2inv, Y);
64 double x1,
double y1,
double z1,
double yaw1,
double pitch1,
65 double roll1,
double xd,
double yd,
double zd,
double yawd,
66 double pitchd,
double rolld,
double x2,
double y2,
double z2,
67 double yaw2,
double pitch2,
double roll2)
69 const CPose3D P1_(x1, y1,
z1, yaw1, pitch1, roll1);
70 const CPose3D Pd_(xd, yd, zd, yawd, pitchd, rolld);
71 const CPose3D P2_(x2, y2,
z2, yaw2, pitch2, roll2);
73 const POSE_TYPE P1(P1_);
74 const POSE_TYPE Pd(Pd_);
75 const POSE_TYPE P2(P2_);
79 P1.getHomogeneousMatrixVal() * Pd.getHomogeneousMatrixVal() *
80 P2.getInverseHomogeneousMatrix()));
81 const POSE_TYPE P1DP2inv(P1DP2inv_);
83 static const int DIMS = SE_TYPE::VECTOR_SIZE;
87 SE_TYPE::jacobian_dP1DP2inv_depsilon(P1DP2inv, &J1, &J2);
93 for (
int i = 0; i < DIMS + DIMS; i++) x_mean[i] = 0;
101 x_incrs.assign(1e-4);
109 x_incrs,
params, numJacobs);
111 numJacobs.extractMatrix(0, 0, num_J1);
112 numJacobs.extractMatrix(0, DIMS, num_J2);
115 const double max_eror = 1e-3;
117 EXPECT_NEAR(0, (num_J1 - J1).array().abs().
sum(), max_eror)
118 <<
"p1: " << P1 << endl
119 <<
"d: " << Pd << endl
120 <<
"p2: " << P2 << endl
123 <<
"Implemented J1:\n" 126 << J1 - num_J1 << endl;
128 EXPECT_NEAR(0, (num_J2 - J2).array().abs().
sum(), max_eror)
129 <<
"p1: " << P1 << endl
130 <<
"d: " << Pd << endl
131 <<
"p2: " << P2 << endl
134 <<
"Implemented J2:\n" 137 << J2 - num_J2 << endl;
142 test_jacobs_P1DP2inv(
143 0, 0, 0,
DEG2RAD(0),
DEG2RAD(0),
DEG2RAD(0), 0, 0, 0,
DEG2RAD(0),
147 test_jacobs_P1DP2inv(
148 0, 0, 0,
DEG2RAD(10),
DEG2RAD(0),
DEG2RAD(0), 0, 0, 0,
DEG2RAD(10),
152 test_jacobs_P1DP2inv(
153 0, 0, 0,
DEG2RAD(10),
DEG2RAD(0),
DEG2RAD(0), 0, 0, 0,
DEG2RAD(12),
157 test_jacobs_P1DP2inv(
158 0, 0, 0,
DEG2RAD(10),
DEG2RAD(20),
DEG2RAD(30), 0, 0, 0,
DEG2RAD(4),
162 test_jacobs_P1DP2inv(
167 test_jacobs_P1DP2inv(
172 test_jacobs_P1DP2inv(
void do_all_jacobs_test()
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
mrpt::poses::SE_traits< POSE_TYPE::rotation_dimensions > SE_TYPE
TEST_F(SE3_traits_tests, SE3_jacobs)
void jacob_numeric_estimate(const VECTORLIKE &x, std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Numerical estimation of the Jacobian of a user-supplied function - this template redirects to mrpt::m...
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
static void func_numeric(const CArrayDouble< 2 *SE_TYPE::VECTOR_SIZE > &x, const TParams ¶ms, CArrayDouble< SE_TYPE::VECTOR_SIZE > &Y)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
SE_traits_tests< mrpt::poses::CPose3D > SE3_traits_tests
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
A helper class for SE(2) and SE(3) geometry-related transformations, on-manifold optimization Jacobia...
A partial specialization of CArrayNumeric for double numbers.
void test_jacobs_P1DP2inv(double x1, double y1, double z1, double yaw1, double pitch1, double roll1, double xd, double yd, double zd, double yawd, double pitchd, double rolld, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
SE_traits_tests< mrpt::poses::CPose2D > SE2_traits_tests
GLenum const GLfloat * params
CMatrixFixedNumeric< double, 4, 4 > CMatrixDouble44