MRPT  2.0.4
LieTraits_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
11 #include <mrpt/math/CMatrixFixed.h>
12 #include <mrpt/math/num_jacobian.h>
13 #include <mrpt/poses/CPose2D.h>
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/Lie/SE.h>
16 #include <Eigen/Dense>
17 
18 using namespace mrpt;
19 using namespace mrpt::poses;
20 using namespace mrpt::math;
21 using namespace std;
22 
23 // List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in
24 // degrees)
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},
39 };
40 
41 template <class POSE_TYPE>
42 class SE_traits_tests : public ::testing::Test
43 {
44  protected:
45  void SetUp() override {}
46  void TearDown() override {}
48 
49  struct TParams
50  {
51  typename SE_TYPE::type P1, D, P2;
52  };
53 
54  template <unsigned int MAT_LEN>
55  struct TParamsMat
56  {
58  };
59 
63  {
64  typename SE_TYPE::tangent_vector eps1, eps2;
65  for (size_t i = 0; i < SE_TYPE::DOFs; i++)
66  {
67  eps1[i] = x[0 + i];
68  eps2[i] = x[SE_TYPE::DOFs + i];
69  }
70 
71  const POSE_TYPE incr1 = SE_TYPE::exp(eps1);
72  const POSE_TYPE incr2 = SE_TYPE::exp(eps2);
73 
74  const POSE_TYPE P1 = params.P1 + incr1;
75  const POSE_TYPE P2 = params.P2 + incr2;
76  const POSE_TYPE& Pd = params.D;
77 
78  CMatrixDouble44 P1_inv_hm, P2hm, Pd_inv_hm;
79  P1.getInverseHomogeneousMatrix(P1_inv_hm);
80  P2.getHomogeneousMatrix(P2hm);
81  Pd.getInverseHomogeneousMatrix(Pd_inv_hm);
82 
83  const CPose3D DinvP1invP2_(CMatrixDouble44(
84  Pd_inv_hm.asEigen() * P1_inv_hm.asEigen() * P2hm.asEigen()));
85  const POSE_TYPE DinvP1invP2(DinvP1invP2_);
86 
87  // Pseudo-logarithm:
88  Y = SE_TYPE::log(DinvP1invP2);
89  }
90 
92  const CPose3D& P1_, const CPose3D& Pd_, const CPose3D& P2_)
93  {
94  const POSE_TYPE P1(P1_);
95  const POSE_TYPE Pd(Pd_);
96  const POSE_TYPE P2(P2_);
97  const POSE_TYPE Pdinv = -Pd;
98 
99  static const int DIMS = SE_TYPE::DOFs;
100 
101  // Theoretical results:
103  SE_TYPE::jacob_dDinvP1invP2_de1e2(Pdinv, P1, P2, J1, J2);
104 
105  // Numerical approx:
106  CMatrixFixed<double, DIMS, DIMS> num_J1, num_J2;
107  {
109  for (int i = 0; i < DIMS + DIMS; i++) x_mean[i] = 0;
110 
111  TParams params;
112  params.P1 = P1;
113  params.D = Pd;
114  params.P2 = P2;
115 
117  x_incrs.fill(1e-4);
118  CMatrixDouble numJacobs;
120  x_mean,
121  std::function<void(
123  const TParams& params,
125  &func_numeric_DinvP1InvP2),
126  x_incrs, params, numJacobs);
127 
128  num_J1 = numJacobs.asEigen().block<DIMS, DIMS>(0, 0);
129  num_J2 = numJacobs.asEigen().block<DIMS, DIMS>(0, DIMS);
130  }
131 
132  const double max_eror = 1e-3;
133 
134  EXPECT_NEAR(0, (num_J1 - J1).sum_abs(), max_eror)
135  << std::setprecision(3) << "p1: " << P1 << endl
136  << "d: " << Pd << endl
137  << "p2: " << P2 << endl
138  << "Numeric J1:\n"
139  << num_J1 << endl
140  << "Implemented J1:\n"
141  << J1 << endl
142  << "Error:\n"
143  << J1 - num_J1 << endl;
144 
145  EXPECT_NEAR(0, (num_J2 - J2).sum_abs(), max_eror)
146  << "p1: " << P1 << endl
147  << "d: " << Pd << endl
148  << "p2: " << P2 << endl
149  << "Numeric J2:\n"
150  << num_J2 << endl
151  << "Implemented J2:\n"
152  << J2 << endl
153  << "Error:\n"
154  << J2 - num_J2 << endl;
155  }
156 
157  static void func_numeric_dAB_dA(
161  {
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);
165  }
166 
167  static void func_numeric_dAB_dB(
171  {
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);
175  }
176 
177  void test_jacobs_AB(const CPose3D& A_, const CPose3D& B_)
178  {
179  const POSE_TYPE A(A_);
180  const POSE_TYPE B(B_);
181 
182  constexpr auto N = SE_TYPE::MANIFOLD_DIM;
183 
184  // Theoretical results:
185  const auto dAB_A = SE_TYPE::jacob_dAB_dA(A, B);
186  const auto dAB_B = SE_TYPE::jacob_dAB_dB(A, B);
187 
188  // Numerical approx: dAB_A
189  CMatrixFixed<double, N, N> num_dAB_A;
190  {
191  CVectorFixedDouble<N> x_mean;
192  x_mean.fill(0);
193 
195  params.Avec = SE_TYPE::asManifoldVector(A);
196  params.Bvec = SE_TYPE::asManifoldVector(B);
197 
198  CVectorFixedDouble<N> x_incrs;
199  x_incrs.fill(1e-4);
200  CMatrixDouble numJacobs;
202  x_mean,
203  std::function<void(
204  const CVectorFixedDouble<N>& x, const TParamsMat<N>& params,
205  CVectorFixedDouble<N>& Y)>(&func_numeric_dAB_dA),
206  x_incrs, params, numJacobs);
207 
208  num_dAB_A = numJacobs;
209  }
210 
211  // Numerical approx: dAB_B
212  CMatrixFixed<double, N, N> num_dAB_B;
213  {
214  CVectorFixedDouble<N> x_mean;
215  x_mean.fill(0);
216 
218  params.Avec = SE_TYPE::asManifoldVector(A);
219  params.Bvec = SE_TYPE::asManifoldVector(B);
220 
221  CVectorFixedDouble<N> x_incrs;
222  x_incrs.fill(1e-4);
223  CMatrixDouble numJacobs;
225  x_mean,
226  std::function<void(
227  const CVectorFixedDouble<N>& x, const TParamsMat<N>& params,
228  CVectorFixedDouble<N>& Y)>(&func_numeric_dAB_dB),
229  x_incrs, params, numJacobs);
230 
231  num_dAB_B = numJacobs;
232  }
233 
234  const double max_eror = 1e-3;
235 
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"
240  << num_dAB_A << endl
241  << "Implemented dAB_A:\n"
242  << dAB_A << endl
243  << "Error:\n"
244  << dAB_A - num_dAB_A << endl;
245 
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"
250  << num_dAB_B << endl
251  << "Implemented dAB_B:\n"
252  << dAB_B << endl
253  << "Error:\n"
254  << dAB_B - num_dAB_B << endl;
255  }
256 
258  {
259  for (const auto& p1 : ptc)
260  for (const auto& p2 : ptc)
261  for (const auto& pd : ptc) test_jacobs_DinvP1InvP2(p1, pd, p2);
262  }
263 
265  {
266  for (const auto& p1 : ptc)
267  for (const auto& p2 : ptc) test_jacobs_AB(p1, p2);
268  }
269 };
270 
273 
274 TEST_F(SE3_traits_tests, SE3_jacobs_DinvP1InvP2) { tests_jacobs_DinvP1InvP2(); }
275 TEST_F(SE3_traits_tests, SE3_jacobs_dAB_dAB) { tests_jacobs_dAB_dAB(); }
276 
277 TEST_F(SE2_traits_tests, SE2_jacobs_DinvP1InvP2) { tests_jacobs_DinvP1InvP2(); }
278 TEST_F(SE2_traits_tests, SE2_jacobs_dAB_dAB) { tests_jacobs_dAB_dAB(); }
TEST_F(SE3_traits_tests, SE3_jacobs_DinvP1InvP2)
void test_jacobs_DinvP1InvP2(const CPose3D &P1_, const CPose3D &Pd_, const CPose3D &P2_)
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
static void func_numeric_dAB_dB(const CVectorFixedDouble< SE_TYPE::MANIFOLD_DIM > &x, const TParamsMat< SE_TYPE::MANIFOLD_DIM > &params, CVectorFixedDouble< SE_TYPE::MANIFOLD_DIM > &Y)
static const std::vector< mrpt::poses::CPose3D > ptc
CVectorFixedDouble< MAT_LEN > Bvec
mrpt::vision::TStereoCalibParams params
STL namespace.
void SetUp() override
This base provides a set of functions for maths stuff.
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...
Definition: num_jacobian.h:31
void TearDown() override
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
static void func_numeric_dAB_dA(const CVectorFixedDouble< SE_TYPE::MANIFOLD_DIM > &x, const TParamsMat< SE_TYPE::MANIFOLD_DIM > &params, CVectorFixedDouble< SE_TYPE::MANIFOLD_DIM > &Y)
static void func_numeric_DinvP1InvP2(const CVectorFixedDouble< 2 *SE_TYPE::DOFs > &x, const TParams &params, CVectorFixedDouble< SE_TYPE::DOFs > &Y)
void test_jacobs_AB(const CPose3D &A_, const CPose3D &B_)
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
CMatrixFixed< double, 4, 4 > CMatrixDouble44
Definition: CMatrixFixed.h:368
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020