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_FLOAT_EQ(p2d.
x(), p2d_bis.
x()) <<
"p2d: " << p2d << endl;
155 EXPECT_FLOAT_EQ(p2d.
y(), p2d_bis.
y()) <<
"p2d: " << p2d << endl;
156 EXPECT_FLOAT_EQ(p2d.
phi(), p2d_bis.
phi()) <<
"p2d: " << p2d << endl;
158 EXPECT_FLOAT_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());
287 for (
int i = 0; i < 3; i++) Y[i] = pp[i];
304 const CPose3D& p1,
double x,
double y,
double z,
bool use_aprox =
false)
313 x,
y,
z, pp.
x, pp.
y, pp.
z, &df_dpoint, &df_dpose,
nullptr,
321 for (
int i = 0; i < 6; i++) x_mean[i] = p1[i];
335 x_incrs, DUMMY, numJacobs);
337 num_df_dpose = numJacobs.
block<3, 6>(0, 0);
338 num_df_dpoint = numJacobs.
block<3, 3>(0, 6);
341 const double max_error = use_aprox ? 0.1 : 3e-3;
343 EXPECT_NEAR(0, (df_dpoint - num_df_dpoint).sum_abs(), max_error)
344 <<
"p1: " << p1 << endl
345 <<
"p: " <<
p << endl
346 <<
"Numeric approximation of df_dpoint: " << endl
347 << num_df_dpoint << endl
348 <<
"Implemented method: " << endl
351 << df_dpoint - num_df_dpoint << endl;
355 (df_dpose.
asEigen() - num_df_dpose.
asEigen()).array().abs().sum(),
357 <<
"p1: " << p1 << endl
358 <<
"p: " <<
p << endl
359 <<
"Numeric approximation of df_dpose: " << endl
360 << num_df_dpose.
asEigen() << endl
361 <<
"Implemented method: " << endl
371 <<
"p1: " << p1 << endl;
375 const CPose3D& p1,
double x,
double y,
double z)
384 x,
y,
z, pp.
x, pp.
y, pp.
z, &df_dpoint, &df_dpose);
391 for (
int i = 0; i < 6; i++) x_mean[i] = p1[i];
405 x_incrs, DUMMY, numJacobs);
407 num_df_dpose = numJacobs.
block<3, 6>(0, 0);
408 num_df_dpoint = numJacobs.
block<3, 3>(0, 6);
412 0, (df_dpoint - num_df_dpoint).asEigen().array().abs().
sum(), 3e-3)
413 <<
"p1: " << p1 << endl
414 <<
"p: " <<
p << endl
415 <<
"Numeric approximation of df_dpoint: " << endl
416 << num_df_dpoint << endl
417 <<
"Implemented method: " << endl
420 << df_dpoint - num_df_dpoint << endl;
424 (df_dpose.
asEigen() - num_df_dpose.
asEigen()).array().abs().sum(),
426 <<
"p1: " << p1 << endl
427 <<
"p: " <<
p << endl
428 <<
"Numeric approximation of df_dpose: " << endl
429 << num_df_dpose.
asEigen() << endl
430 <<
"Implemented method: " << endl
441 EXPECT_EQ(
p.yaw(), 0);
442 EXPECT_EQ(
p.pitch(), 0);
443 EXPECT_EQ(
p.roll(), 0);
444 for (
size_t i = 0; i < 4; i++)
445 for (
size_t j = 0; j < 4; j++)
448 i == j ? 1.0 : 0.0, 1e-8)
449 <<
"Failed for (i,j)=" << i <<
"," << j << endl
450 <<
"Matrix is: " << endl
452 <<
"case was: " << label << endl;
462 for (
int i = 0; i < 3; i++) Y[i] = pp[i];
472 for (
int i = 0; i < 3; i++) Y[i] = pp[i];
481 x_l.
x, x_l.
y, x_l.
z, pp.
x, pp.
y, pp.
z,
nullptr,
nullptr, &df_dse3);
487 for (
int i = 0; i < 6; i++) x_mean[i] = 0;
490 for (
int i = 0; i < 3; i++) P[i] = pp[i];
499 &func_compose_point_se3),
500 x_incrs, P, num_df_dse3);
504 0, (df_dse3.
asEigen() - num_df_dse3.
asEigen()).array().abs().sum(),
506 <<
"p: " <<
p << endl
507 <<
"x_l: " << x_l << endl
508 <<
"Numeric approximation of df_dse3: " << endl
509 << num_df_dse3.
asEigen() << endl
510 <<
"Implemented method: " << endl
521 p.inverseComposePoint(
522 x_g.
x, x_g.
y, x_g.
z, pp.
x, pp.
y, pp.
z,
nullptr,
nullptr, &df_dse3);
528 for (
int i = 0; i < 6; i++) x_mean[i] = 0;
531 for (
int i = 0; i < 3; i++) P[i] = pp[i];
540 &func_invcompose_point_se3),
541 x_incrs, P, num_df_dse3);
545 0, (df_dse3.
asEigen() - num_df_dse3.
asEigen()).array().abs().sum(),
547 <<
"p: " <<
p << endl
548 <<
"x_g: " << x_g << endl
549 <<
"Numeric approximation of df_dse3: " << endl
550 << num_df_dse3.
asEigen() << endl
551 <<
"Implemented method: " << endl
572 for (
int i = 0; i < 6; i++) x_mean[i] = 0;
583 x_incrs, dummy, numJacobs);
590 double vals[12 * 6] = {
591 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0,
593 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
595 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0,
597 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0};
604 << M.
asEigen() <<
"numJacobs:\n" 605 << numJacobs <<
"\n";
615 p.setFrom12Vector(
x);
617 auto R =
p.getRotationMatrix();
619 R.asEigen().jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
620 R = Rsvd.matrixU() * Rsvd.matrixV().transpose();
621 p.setRotationMatrix(
R);
634 p.getAs12Vector(x_mean);
644 x_incrs, dummy, numJacobs);
648 (numJacobs.
asEigen() - theor_jacob.asEigen()).array().abs().sum(),
650 <<
"Pose: " <<
p << endl
655 << theor_jacob.
asEigen() << endl
657 << theor_jacob.asEigen() - numJacobs.
asEigen() << endl;
665 const CPose3D expe_D = incr + D;
687 x_incrs,
p, numJacobs);
691 (numJacobs.
asEigen() - theor_jacob.asEigen())
696 <<
"Pose: " <<
p << endl
701 << theor_jacob.
asEigen() << endl
703 << theor_jacob.asEigen() - numJacobs.
asEigen() << endl;
711 const CPose3D expe_D = D + incr;
733 x_incrs,
p, numJacobs);
737 (numJacobs.
asEigen() - theor_jacob.asEigen())
742 <<
"Pose: " <<
p << endl
747 << theor_jacob.
asEigen() << endl
749 << theor_jacob.asEigen() - numJacobs.
asEigen() << endl;
789 x_incrs,
params, numJacobs);
793 (numJacobs.
asEigen() - theor_jacob.asEigen())
798 <<
"Pose A: " <<
A << endl
799 <<
"Pose D: " << D << endl
803 << theor_jacob.
asEigen() << endl
805 << theor_jacob.asEigen() - numJacobs.
asEigen() << endl;
814 test_default_values(
p,
"Default");
819 test_default_values(
p,
"p=p2");
823 test_default_values(p1 + p2,
"p1+p2");
825 test_default_values(
p,
"p=p1+p2");
830 test_default_values(
p,
"p1-p2");
837 EXPECT_NEAR(
p.x(), 1, 1e-7);
838 EXPECT_NEAR(
p.y(), 2, 1e-7);
839 EXPECT_NEAR(
p.z(), 3, 1e-7);
840 EXPECT_NEAR(
p.yaw(), 0.2, 1e-7);
841 EXPECT_NEAR(
p.pitch(), 0.3, 1e-7);
842 EXPECT_NEAR(
p.roll(), 0.4, 1e-7);
848 EXPECT_NEAR(
p[0], 1, 1e-7);
849 EXPECT_NEAR(
p[1], 2, 1e-7);
850 EXPECT_NEAR(
p[2], 3, 1e-7);
851 EXPECT_NEAR(
p[3], 0.2, 1e-7);
852 EXPECT_NEAR(
p[4], 0.3, 1e-7);
853 EXPECT_NEAR(
p[5], 0.4, 1e-7);
858 static const std::vector<mrpt::poses::CPose3D>
ptc = {
879 for (
const auto&
p :
ptc) test_inverse(
p);
884 for (
const auto& p1 :
ptc)
885 for (
const auto& p2 :
ptc) test_compose(p1, p2);
889 for (
const auto& p1 :
ptc)
890 for (
const auto& p2 :
ptc) test_composeFrom(p1, p2);
895 for (
const auto&
p :
ptc) test_to_from_2d(
p.x(),
p.y(),
p.yaw());
900 for (
const auto&
p :
ptc)
902 test_composePoint(
p, 10, 11, 12);
903 test_composePoint(
p, -5, 1, 2);
904 test_composePoint(
p, 5, -1, 2);
905 test_composePoint(
p, 5, 1, -2);
911 for (
const auto&
p :
ptc)
913 test_composePointJacob(
p, 10, 11, 12);
914 test_composePointJacob(
p, -5, 1, 2);
920 test_composePointJacob(
923 test_composePointJacob(
927 test_composePointJacob(
931 test_composePointJacob(
939 for (
const auto&
p :
ptc)
941 test_invComposePointJacob(
p, 10, 11, 12);
942 test_invComposePointJacob(
p, -5, 1, 2);
943 test_invComposePointJacob(
p, 5, -1, 2);
944 test_invComposePointJacob(
p, 5, 1, -2);
950 for (
const auto&
p :
ptc)
952 test_composePointJacob_se3(
p,
TPoint3D(0, 0, 0));
953 test_composePointJacob_se3(
p,
TPoint3D(10, 11, 12));
954 test_composePointJacob_se3(
p,
TPoint3D(-5.0, -15.0, 8.0));
959 for (
const auto&
p :
ptc)
961 test_invComposePointJacob_se3(
p,
TPoint3D(0, 0, 0));
962 test_invComposePointJacob_se3(
p,
TPoint3D(10, 11, 12));
963 test_invComposePointJacob_se3(
p,
TPoint3D(-5.0, -15.0, 8.0));
969 for (
const auto&
p :
ptc) test_ExpLnEqual(
p);
988 for (
const auto&
p :
ptc) test_Jacob_dexpeD_de(
p);
993 for (
const auto&
p :
ptc) test_Jacob_dDexpe_de(
p);
998 for (
const auto& p1 :
ptc)
999 for (
const auto& p2 :
ptc) test_Jacob_dAexpeD_de(p1, p2);
A compile-time fixed-size numeric matrix container.
static void func_compose_point_se3(const CVectorFixedDouble< 6 > &x, const CVectorFixedDouble< 3 > &P, CVectorFixedDouble< 3 > &Y)
TEST_F(Pose3DTests, DefaultValues)
double x
X,Y,Z coordinates.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
void test_invComposePointJacob_se3(const CPose3D &p, const TPoint3D x_g)
static const std::vector< mrpt::poses::CPose3D > ptc
double DEG2RAD(const double x)
Degrees to radians.
GLdouble GLdouble GLdouble GLdouble q
void test_inverse(const CPose3D &p1)
void test_composePointJacob_se3(const CPose3D &p, const TPoint3D x_l)
void test_invComposePointJacob(const CPose3D &p1, double x, double y, double z)
void test_Jacob_dexpeD_de(const CPose3D &p)
static void func_jacob_expe_D(const CVectorFixedDouble< 6 > &eps, const CPose3D &D, CVectorFixedDouble< 12 > &Y)
static void func_inv_compose_point(const CVectorFixedDouble< 6+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
void test_composePoint(const CPose3D &p1, double x, double y, double z)
double yaw() const
Get the YAW angle (in radians)
void inverse()
Convert this pose into its inverse, saving the result in itself.
void test_to_from_2d(double x, double y, double phi)
static void func_jacob_Aexpe_D(const CVectorFixedDouble< 6 > &eps, const TParams_func_jacob_Aexpe_D ¶ms, CVectorFixedDouble< 12 > &Y)
void check_jacob_expe_e_at_0()
static void func_jacob_LnT_T(const CVectorFixedDouble< 12 > &x, const double &dummy, CVectorFixedDouble< 6 > &Y)
This base provides a set of functions for maths stuff.
void test_ExpLnEqual(const CPose3D &p1)
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixed< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
void test_Jacob_dAexpeD_de(const CPose3D &A, const CPose3D &D)
void getInverseHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
void test_composePointJacob(const CPose3D &p1, double x, double y, double z, bool use_aprox=false)
static void func_invcompose_point_se3(const CVectorFixedDouble< 6 > &x, const CVectorFixedDouble< 3 > &P, CVectorFixedDouble< 3 > &Y)
double x() const
Common members of all points & poses classes.
GLsizei const GLchar ** string
A class used to store a 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
void test_compose(const CPose3D &p1, const CPose3D &p2)
Traits for SE(n), rigid-body transformations in R^n space.
void test_Jacob_dDexpe_de(const CPose3D &p)
void check_jacob_LnT_T(const CPose3D &p)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
const double & phi() const
Get the phi angle of the 2D pose (in radians)
static void func_jacob_D_expe(const CVectorFixedDouble< 6 > &eps, const CPose3D &D, CVectorFixedDouble< 12 > &Y)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
static void func_compose_point(const CVectorFixedDouble< 6+3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
void test_composeFrom(const CPose3D &p1, const CPose3D &p2)
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
void getAs12Vector(ARRAYORVECTOR &vec12) const
Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 ...
GLenum const GLfloat * params
static void func_jacob_expe_e(const CVectorFixedDouble< 6 > &x, const double &dummy, CVectorFixedDouble< 12 > &Y)
void test_default_values(const CPose3D &p, const std::string &label)
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.