Main MRPT website > C++ reference for MRPT 1.9.9
SE_traits_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
11 #include <mrpt/poses/CPose2D.h>
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/poses/SE_traits.h>
14 #include <mrpt/math/num_jacobian.h>
15 #include <gtest/gtest.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::poses;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 template <class POSE_TYPE>
23 class SE_traits_tests : public ::testing::Test
24 {
25  protected:
26  virtual void SetUp() {}
27  virtual void TearDown() {}
29 
30  struct TParams
31  {
32  typename SE_TYPE::pose_t P1, D, P2;
33  };
34 
35  static void func_numeric(
38  {
39  typename SE_TYPE::array_t eps1, eps2;
40  for (int i = 0; i < SE_TYPE::VECTOR_SIZE; i++)
41  {
42  eps1[i] = x[0 + i];
43  eps2[i] = x[SE_TYPE::VECTOR_SIZE + i];
44  }
45 
46  POSE_TYPE incr1, incr2;
47  SE_TYPE::exp(eps1, incr1);
48  SE_TYPE::exp(eps2, incr2);
49 
50  const POSE_TYPE P1 = incr1 + params.P1;
51  const POSE_TYPE P2 = incr2 + params.P2;
52  const POSE_TYPE& Pd = params.D;
53 
54  CMatrixDouble44 HM_P2inv, P1hm, Pdhm;
55  P1.getHomogeneousMatrix(P1hm);
56  Pd.getHomogeneousMatrix(Pdhm);
57  P2.getInverseHomogeneousMatrix(HM_P2inv);
58 
59  const CPose3D P1DP2inv_(CMatrixDouble44(P1hm * Pdhm * HM_P2inv));
60  const POSE_TYPE P1DP2inv(P1DP2inv_);
61 
62  // Pseudo-logarithm:
63  SE_TYPE::pseudo_ln(P1DP2inv, Y);
64  }
65 
67  double x1, double y1, double z1, double yaw1, double pitch1,
68  double roll1, double xd, double yd, double zd, double yawd,
69  double pitchd, double rolld, double x2, double y2, double z2,
70  double yaw2, double pitch2, double roll2)
71  {
72  const CPose3D P1_(x1, y1, z1, yaw1, pitch1, roll1);
73  const CPose3D Pd_(xd, yd, zd, yawd, pitchd, rolld);
74  const CPose3D P2_(x2, y2, z2, yaw2, pitch2, roll2);
75 
76  const POSE_TYPE P1(P1_);
77  const POSE_TYPE Pd(Pd_);
78  const POSE_TYPE P2(P2_);
79 
80  CMatrixDouble44 HM_P2inv, P1hm, Pdhm;
81  P1.getHomogeneousMatrix(P1hm);
82  Pd.getHomogeneousMatrix(Pdhm);
83  P2.getInverseHomogeneousMatrix(HM_P2inv);
84  const CPose3D P1DP2inv_(CMatrixDouble44((P1hm * Pdhm) * HM_P2inv));
85  const POSE_TYPE P1DP2inv(P1DP2inv_); // Convert to 2D if needed
86 
87  static const int DIMS = SE_TYPE::VECTOR_SIZE;
88 
89  // Theoretical results:
91  SE_TYPE::jacobian_dP1DP2inv_depsilon(P1DP2inv, &J1, &J2);
92 
93  // Numerical approx:
95  {
97  for (int i = 0; i < DIMS + DIMS; i++) x_mean[i] = 0;
98 
100  params.P1 = P1;
101  params.D = Pd;
102  params.P2 = P2;
103 
105  x_incrs.assign(1e-4);
106  CMatrixDouble numJacobs;
108  x_mean,
109  std::function<void(
111  const TParams& params,
112  CArrayDouble<SE_TYPE::VECTOR_SIZE>& Y)>(&func_numeric),
113  x_incrs, params, numJacobs);
114 
115  numJacobs.extractMatrix(0, 0, num_J1);
116  numJacobs.extractMatrix(0, DIMS, num_J2);
117  }
118 
119  const double max_eror = 1e-3;
120 
121  EXPECT_NEAR(0, (num_J1 - J1).array().abs().sum(), max_eror)
122  << "p1: " << P1 << endl
123  << "d: " << Pd << endl
124  << "p2: " << P2 << endl
125  << "Numeric J1:\n"
126  << num_J1 << endl
127  << "Implemented J1:\n"
128  << J1 << endl
129  << "Error:\n"
130  << J1 - num_J1 << endl;
131 
132  EXPECT_NEAR(0, (num_J2 - J2).array().abs().sum(), max_eror)
133  << "p1: " << P1 << endl
134  << "d: " << Pd << endl
135  << "p2: " << P2 << endl
136  << "Numeric J2:\n"
137  << num_J2 << endl
138  << "Implemented J2:\n"
139  << J2 << endl
140  << "Error:\n"
141  << J2 - num_J2 << endl;
142  }
143 
145  {
146  test_jacobs_P1DP2inv(
147  0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0),
148  DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(0), DEG2RAD(0),
149  DEG2RAD(0));
150 
151  test_jacobs_P1DP2inv(
152  0, 0, 0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(10),
153  DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(20), DEG2RAD(0),
154  DEG2RAD(0));
155 
156  test_jacobs_P1DP2inv(
157  0, 0, 0, DEG2RAD(10), DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(12),
158  DEG2RAD(0), DEG2RAD(0), 0, 0, 0, DEG2RAD(20), DEG2RAD(0),
159  DEG2RAD(0));
160 
161  test_jacobs_P1DP2inv(
162  0, 0, 0, DEG2RAD(10), DEG2RAD(20), DEG2RAD(30), 0, 0, 0, DEG2RAD(4),
163  DEG2RAD(3), DEG2RAD(8), 0, 0, 0, DEG2RAD(15), DEG2RAD(25),
164  DEG2RAD(35));
165 
166  test_jacobs_P1DP2inv(
167  10, 20, 30, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), -5, -5, -5,
168  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 5, 15, 25, DEG2RAD(0),
169  DEG2RAD(0), DEG2RAD(0));
170 
171  test_jacobs_P1DP2inv(
172  10, 20, 30, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), -5.2, -5.3, -4.9,
173  DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 5, 15, 25, DEG2RAD(0),
174  DEG2RAD(0), DEG2RAD(0));
175 
176  test_jacobs_P1DP2inv(
177  10, 20, 30, DEG2RAD(10), DEG2RAD(20), DEG2RAD(30), 4.7, 4.8, 5.3,
178  DEG2RAD(4), DEG2RAD(3), DEG2RAD(8), 15, 25, 35, DEG2RAD(15),
179  DEG2RAD(25), DEG2RAD(35));
180  }
181 };
182 
185 
186 TEST_F(SE3_traits_tests, SE3_jacobs) { do_all_jacobs_test(); }
187 TEST_F(SE2_traits_tests, SE2_jacobs) { do_all_jacobs_test(); }
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
SE_traits_tests::TParams
Definition: SE_traits_unittest.cpp:30
SE_traits_tests::SetUp
virtual void SetUp()
Definition: SE_traits_unittest.cpp:26
CMatrixFixedNumeric.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
CPose2D.h
SE_traits_tests::do_all_jacobs_test
void do_all_jacobs_test()
Definition: SE_traits_unittest.cpp:144
mrpt::poses::SE_traits
A helper class for SE(2) and SE(3) geometry-related transformations, on-manifold optimization Jacobia...
Definition: SE_traits.h:29
mrpt::math::CMatrixTemplateNumeric< double >
SE_traits_tests
Definition: SE_traits_unittest.cpp:23
num_jacobian.h
SE_traits_tests::TParams::P2
SE_TYPE::pose_t P2
Definition: SE_traits_unittest.cpp:32
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
SE_traits_tests::TearDown
virtual void TearDown()
Definition: SE_traits_unittest.cpp:27
SE_traits.h
SE_traits_tests::func_numeric
static void func_numeric(const CArrayDouble< 2 *SE_TYPE::VECTOR_SIZE > &x, const TParams &params, CArrayDouble< SE_TYPE::VECTOR_SIZE > &Y)
Definition: SE_traits_unittest.cpp:35
mrpt::math::CArrayNumeric
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
Definition: CArrayNumeric.h:25
SE_traits_tests::test_jacobs_P1DP2inv
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)
Definition: SE_traits_unittest.cpp:66
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:40
CPose3D.h
mrpt::math::CMatrixDouble44
CMatrixFixedNumeric< double, 4, 4 > CMatrixDouble44
Definition: eigen_frwds.h:58
TEST_F
TEST_F(SE3_traits_tests, SE3_jacobs)
Definition: SE_traits_unittest.cpp:186
mrpt::math::estimateJacobian
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...
Definition: num_jacobian.h:31
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
x
GLenum GLint x
Definition: glext.h:3538
params
GLenum const GLfloat * params
Definition: glext.h:3534
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST