Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
10 #include <mrpt/poses/CPose3D.h>
11 #include <mrpt/poses/SE_traits.h>
12 #include <mrpt/math/jacobians.h>
13 #include <gtest/gtest.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::poses;
17 using namespace mrpt::utils;
18 using namespace mrpt::math;
19 using namespace std;
20 
21 
22 template <class POSE_TYPE>
23 class SE_traits_tests : public ::testing::Test {
24 protected:
25  virtual void SetUp()
26  {
27  }
28  virtual void TearDown() { }
29 
30  typedef mrpt::poses::SE_traits<POSE_TYPE::rotation_dimensions> SE_TYPE;
31 
32  struct TParams
33  {
34  typename SE_TYPE::pose_t P1, D, P2;
35  };
36 
37  static void func_numeric(
39  const TParams &params,
41  {
42  typename SE_TYPE::array_t eps1, eps2;
43  for (int i=0;i<SE_TYPE::VECTOR_SIZE;i++)
44  {
45  eps1[i] = x[0+i];
46  eps2[i] = x[SE_TYPE::VECTOR_SIZE+i];
47  }
48 
49  POSE_TYPE incr1, incr2;
50  SE_TYPE::exp(eps1,incr1);
51  SE_TYPE::exp(eps2,incr2);
52 
53  const POSE_TYPE P1 = incr1 + params.P1;
54  const POSE_TYPE P2 = incr2 + params.P2;
55  const POSE_TYPE &Pd = params.D;
56 
57  const CPose3D P1DP2inv_(CMatrixDouble44(P1.getHomogeneousMatrixVal() * Pd.getHomogeneousMatrixVal() * P2.getInverseHomogeneousMatrix()));
58  const POSE_TYPE P1DP2inv(P1DP2inv_);
59 
60  // Pseudo-logarithm:
61  SE_TYPE::pseudo_ln(P1DP2inv,Y);
62  }
63 
65  double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
66  double xd,double yd,double zd, double yawd,double pitchd,double rolld,
67  double x2,double y2,double z2, double yaw2,double pitch2,double roll2)
68  {
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);
72 
73  const POSE_TYPE P1(P1_);
74  const POSE_TYPE Pd(Pd_);
75  const POSE_TYPE P2(P2_);
76 
77  const CPose3D P1DP2inv_( CMatrixDouble44(P1.getHomogeneousMatrixVal() * Pd.getHomogeneousMatrixVal() * P2.getInverseHomogeneousMatrix()));
78  const POSE_TYPE P1DP2inv(P1DP2inv_); // Convert to 2D if needed
79 
80  static const int DIMS = SE_TYPE::VECTOR_SIZE;
81 
82  // Theoretical results:
84  SE_TYPE::jacobian_dP1DP2inv_depsilon(P1DP2inv, &J1, &J2);
85 
86  // Numerical approx:
88  {
89  CArrayDouble<2*DIMS> x_mean;
90  for (int i=0;i<DIMS+DIMS;i++) x_mean[i]=0;
91 
93  params.P1 = P1;
94  params.D = Pd;
95  params.P2 = P2;
96 
98  x_incrs.assign(1e-4);
99  CMatrixDouble numJacobs;
100  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_numeric,x_incrs, params, numJacobs );
101 
102  numJacobs.extractMatrix(0,0, num_J1);
103  numJacobs.extractMatrix(0,DIMS, num_J2);
104  }
105 
106  const double max_eror = 1e-3;
107 
108  EXPECT_NEAR(0, (num_J1-J1).array().abs().sum(), max_eror )
109  << "p1: " << P1 << endl
110  << "d: " << Pd << endl
111  << "p2: " << P2 << endl
112  << "Numeric J1:\n" << num_J1 << endl
113  << "Implemented J1:\n" << J1 << endl
114  << "Error:\n" << J1-num_J1 << endl;
115 
116  EXPECT_NEAR(0, (num_J2-J2).array().abs().sum(), max_eror )
117  << "p1: " << P1 << endl
118  << "d: " << Pd << endl
119  << "p2: " << P2 << endl
120  << "Numeric J2:\n" << num_J2 << endl
121  << "Implemented J2:\n" << J2 << endl
122  << "Error:\n" << J2-num_J2 << endl;
123  }
124 
125 
127  {
128  test_jacobs_P1DP2inv(
129  0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0),
130  0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0),
131  0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
132 
133  test_jacobs_P1DP2inv(
134  0,0,0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0),
135  0,0,0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0),
136  0,0,0, DEG2RAD(20),DEG2RAD(0),DEG2RAD(0) );
137 
138  test_jacobs_P1DP2inv(
139  0,0,0, DEG2RAD(10),DEG2RAD(0),DEG2RAD(0),
140  0,0,0, DEG2RAD(12),DEG2RAD(0),DEG2RAD(0),
141  0,0,0, DEG2RAD(20),DEG2RAD(0),DEG2RAD(0) );
142 
143  test_jacobs_P1DP2inv(
144  0,0,0, DEG2RAD(10),DEG2RAD(20),DEG2RAD(30),
145  0,0,0, DEG2RAD(4),DEG2RAD(3),DEG2RAD(8),
146  0,0,0, DEG2RAD(15),DEG2RAD(25),DEG2RAD(35) );
147 
148  test_jacobs_P1DP2inv(
149  10,20,30, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0),
150  -5,-5,-5, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0),
151  5,15,25, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
152 
153  test_jacobs_P1DP2inv(
154  10,20,30, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0),
155  -5.2,-5.3,-4.9, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0),
156  5,15,25, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
157 
158  test_jacobs_P1DP2inv(
159  10,20,30, DEG2RAD(10),DEG2RAD(20),DEG2RAD(30),
160  4.7,4.8,5.3, DEG2RAD(4),DEG2RAD(3),DEG2RAD(8),
161  15,25,35, DEG2RAD(15),DEG2RAD(25),DEG2RAD(35) );
162  }
163 };
164 
167 
169 {
170  do_all_jacobs_test();
171 }
172 
174 {
175  do_all_jacobs_test();
176 }
virtual void SetUp()
mrpt::poses::SE_traits< POSE_TYPE::rotation_dimensions > SE_TYPE
TEST_F(SE3_traits_tests, SE3_jacobs)
CMatrixFixedNumeric< double, 4, 4 > CMatrixDouble44
Definition: eigen_frwds.h:49
A numeric matrix of compile-time fixed size.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
virtual void TearDown()
#define DEG2RAD
static void func_numeric(const CArrayDouble< 2 *SE_TYPE::VECTOR_SIZE > &x, const TParams &params, CArrayDouble< SE_TYPE::VECTOR_SIZE > &Y)
SE_traits_tests< mrpt::poses::CPose3D > SE3_traits_tests
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
GLfloat * params
Definition: glew.h:1436
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
void jacob_numeric_estimate(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), 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...
Definition: jacobians.h:119
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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018