Main MRPT website > C++ reference for MRPT 1.5.9
CPose3DQuatPDFGaussian_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/CPose3DPDFGaussian.h>
11 #include <mrpt/poses/CPose3DQuatPDFGaussian.h>
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/random.h>
14 #include <mrpt/math/transform_gaussian.h>
15 #include <mrpt/math/jacobians.h>
16 #include <gtest/gtest.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::poses;
20 using namespace mrpt::utils;
21 using namespace mrpt::math;
22 using namespace std;
23 
24 
25 
26 class Pose3DQuatPDFGaussTests : public ::testing::Test {
27 protected:
28  virtual void SetUp()
29  {
30  }
31 
32  virtual void TearDown() { }
33 
34  static CPose3DQuatPDFGaussian generateRandomPoseQuat3DPDF(double x,double y, double z, double yaw, double pitch, double roll, double std_scale)
35  {
36  return CPose3DQuatPDFGaussian(generateRandomPose3DPDF(x,y,z,yaw,pitch,roll,std_scale));
37  }
38 
39  static CPose3DPDFGaussian generateRandomPose3DPDF(double x,double y, double z, double yaw, double pitch, double roll, double std_scale)
40  {
41  CMatrixDouble61 r;
42  mrpt::random::randomGenerator.drawGaussian1DMatrix(r, 0,std_scale);
43  CMatrixDouble66 cov;
44  cov.multiply_AAt(r); // random semi-definite positive matrix:
45  for (int i=0;i<6;i++) cov(i,i)+=1e-7;
46  CPose3DPDFGaussian p6pdf( CPose3D(x,y,z,yaw,pitch,roll), cov );
47  return p6pdf;
48  }
49 
50 
51  void test_toFromYPRGauss(double yaw, double pitch,double roll)
52  {
53  // Random pose:
54  CPose3DPDFGaussian p1ypr = generateRandomPose3DPDF(1.0,2.0,3.0, yaw,pitch,roll, 0.1);
55  CPose3DQuatPDFGaussian p1quat = CPose3DQuatPDFGaussian(p1ypr);
56 
57  // Convert back to a 6x6 representation:
58  CPose3DPDFGaussian p2ypr = CPose3DPDFGaussian(p1quat);
59 
60  EXPECT_NEAR(0,(p2ypr.cov - p1ypr.cov).array().abs().mean(), 1e-2)
61  << "p1ypr: " << endl << p1ypr << endl
62  << "p1quat : " << endl << p1quat << endl
63  << "p2ypr : " << endl << p2ypr << endl;
64  }
65 
66  static void func_compose(const CArrayDouble<2*7> &x, const double &dummy, CArrayDouble<7> &Y)
67  {
68  MRPT_UNUSED_PARAM(dummy);
69  const CPose3DQuat p1(x[0],x[1],x[2],CQuaternionDouble(x[3],x[4],x[5],x[6]));
70  const CPose3DQuat p2(x[7+0],x[7+1],x[7+2],CQuaternionDouble(x[7+3],x[7+4],x[7+5],x[7+6]));
71  const CPose3DQuat p = p1+p2;
72  for (int i=0;i<7;i++) Y[i]=p[i];
73  }
74 
75  static void func_inv_compose(const CArrayDouble<2*7> &x, const double &dummy, CArrayDouble<7> &Y)
76  {
77  MRPT_UNUSED_PARAM(dummy);
78  const CPose3DQuat p1(x[0],x[1],x[2],CQuaternionDouble(x[3],x[4],x[5],x[6]));
79  const CPose3DQuat p2(x[7+0],x[7+1],x[7+2],CQuaternionDouble(x[7+3],x[7+4],x[7+5],x[7+6]));
80  const CPose3DQuat p = p1-p2;
81  for (int i=0;i<7;i++) Y[i]=p[i];
82  }
83 
84  void testPoseComposition(
85  double x,double y, double z, double yaw, double pitch, double roll, double std_scale,
86  double x2,double y2, double z2, double yaw2, double pitch2, double roll2, double std_scale2 )
87  {
88  CPose3DQuatPDFGaussian p7pdf1 = generateRandomPoseQuat3DPDF(x,y,z,yaw,pitch,roll, std_scale);
89  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(x2,y2,z2,yaw2,pitch2,roll2, std_scale2);
90 
91  CPose3DQuatPDFGaussian p7_comp = p7pdf1 + p7pdf2;
92 
93  // Numeric approximation:
94  CArrayDouble<7> y_mean;
95  CMatrixFixedNumeric<double,7,7> y_cov;
96  {
97  CArrayDouble<2*7> x_mean;
98  for (int i=0;i<7;i++) x_mean[i]=p7pdf1.mean[i];
99  for (int i=0;i<7;i++) x_mean[7+i]=p7pdf2.mean[i];
100 
101  CMatrixFixedNumeric<double,14,14> x_cov;
102  x_cov.insertMatrix(0,0, p7pdf1.cov);
103  x_cov.insertMatrix(7,7, p7pdf2.cov);
104 
105  double DUMMY=0;
106  CArrayDouble<2*7> x_incrs;
107  x_incrs.assign(1e-6);
108  transform_gaussian_linear(x_mean,x_cov,func_compose,DUMMY, y_mean,y_cov, x_incrs );
109 
110  }
111  // Compare:
112  EXPECT_NEAR(0, (y_cov-p7_comp.cov).array().abs().mean(), 1e-2 )
113  << "p1 mean: " << p7pdf1.mean << endl
114  << "p2 mean: " << p7pdf2.mean << endl
115  << "Numeric approximation of covariance: " << endl << y_cov << endl
116  << "Returned covariance: " << endl << p7_comp.cov << endl;
117 
118  }
119 
120  static void func_inverse(const CArrayDouble<7> &x, const double &dummy, CArrayDouble<7> &Y)
121  {
122  MRPT_UNUSED_PARAM(dummy);
123  const CPose3DQuat p1(x[0],x[1],x[2],CQuaternionDouble(x[3],x[4],x[5],x[6]));
124  const CPose3DQuat p1_inv ( -p1 );
125  for (int i=0;i<7;i++) Y[i]=p1_inv[i];
126  }
127 
128  void testCompositionJacobian(
129  double x,double y, double z, double yaw, double pitch, double roll,
130  double x2,double y2, double z2, double yaw2, double pitch2, double roll2)
131  {
132  const CPose3DQuat q1( CPose3D(x,y,z,yaw,pitch,roll) );
133  const CPose3DQuat q2( CPose3D(x2,y2,z2,yaw2,pitch2,roll2) );
134 
135  // Theoretical Jacobians:
136  CMatrixDouble77 df_dx(UNINITIALIZED_MATRIX), df_du(UNINITIALIZED_MATRIX);
137  CPose3DQuatPDF::jacobiansPoseComposition(
138  q1, // x
139  q2, // u
140  df_dx,
141  df_du );
142 
143  // Numerical approximation:
144  CMatrixDouble77 num_df_dx(UNINITIALIZED_MATRIX), num_df_du(UNINITIALIZED_MATRIX);
145  {
146  CArrayDouble<2*7> x_mean;
147  for (int i=0;i<7;i++) x_mean[i]=q1[i];
148  for (int i=0;i<7;i++) x_mean[7+i]=q2[i];
149 
150  double DUMMY=0;
151  CArrayDouble<2*7> x_incrs;
152  x_incrs.assign(1e-7);
153  CMatrixDouble numJacobs;
154  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose,x_incrs, DUMMY, numJacobs );
155 
156  numJacobs.extractMatrix(0,0, num_df_dx);
157  numJacobs.extractMatrix(0,7, num_df_du);
158  }
159 
160  // Compare:
161  EXPECT_NEAR(0, (df_dx-num_df_dx).array().abs().sum(), 3e-3 )
162  << "q1: " << q1 << endl
163  << "q2: " << q2 << endl
164  << "Numeric approximation of df_dx: " << endl << num_df_dx << endl
165  << "Implemented method: " << endl << df_dx << endl
166  << "Error: " << endl << df_dx-num_df_dx << endl;
167 
168  EXPECT_NEAR(0, (df_du-num_df_du).array().abs().sum(), 3e-3 )
169  << "q1: " << q1 << endl
170  << "q2: " << q2 << endl
171  << "Numeric approximation of df_du: " << endl << num_df_du << endl
172  << "Implemented method: " << endl << df_du << endl
173  << "Error: " << endl << df_du-num_df_du << endl;
174  }
175 
176  void testInverse(double x,double y, double z, double yaw, double pitch, double roll, double std_scale)
177  {
178  CPose3DQuatPDFGaussian p7pdf1 = generateRandomPoseQuat3DPDF(x,y,z,yaw,pitch,roll, std_scale);
179 
180  CPose3DQuatPDFGaussian p7_inv = -p7pdf1;
181 
182  // Numeric approximation:
183  CArrayDouble<7> y_mean;
184  CMatrixFixedNumeric<double,7,7> y_cov;
185  {
186  CArrayDouble<7> x_mean;
187  for (int i=0;i<7;i++) x_mean[i]=p7pdf1.mean[i];
188 
189  CMatrixFixedNumeric<double,7,7> x_cov;
190  x_cov.insertMatrix(0,0, p7pdf1.cov);
191 
192  double DUMMY=0;
193  CArrayDouble<7> x_incrs;
194  x_incrs.assign(1e-6);
195  transform_gaussian_linear(x_mean,x_cov,func_inverse,DUMMY, y_mean,y_cov, x_incrs );
196  }
197 
198  // Compare:
199  EXPECT_NEAR(0, (y_cov-p7_inv.cov).array().abs().mean(), 1e-2 )
200  << "p1 mean: " << p7pdf1.mean << endl
201  << "inv mean: " << p7_inv.mean << endl
202  << "Numeric approximation of covariance: " << endl << y_cov << endl
203  << "Returned covariance: " << endl << p7_inv.cov << endl
204  << "Error: " << endl << y_cov-p7_inv.cov << endl;
205  }
206 
207 
208  void testPoseInverseComposition(
209  double x,double y, double z, double yaw, double pitch, double roll, double std_scale,
210  double x2,double y2, double z2, double yaw2, double pitch2, double roll2, double std_scale2 )
211  {
212  CPose3DQuatPDFGaussian p7pdf1 = generateRandomPoseQuat3DPDF(x,y,z,yaw,pitch,roll, std_scale);
213  CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(x2,y2,z2,yaw2,pitch2,roll2, std_scale2);
214 
215  CPose3DQuatPDFGaussian p7_comp = p7pdf1 - p7pdf2;
216 
217  // Numeric approximation:
218  CArrayDouble<7> y_mean;
219  CMatrixFixedNumeric<double,7,7> y_cov;
220  {
221  CArrayDouble<2*7> x_mean;
222  for (int i=0;i<7;i++) x_mean[i]=p7pdf1.mean[i];
223  for (int i=0;i<7;i++) x_mean[7+i]=p7pdf2.mean[i];
224 
225  CMatrixFixedNumeric<double,14,14> x_cov;
226  x_cov.insertMatrix(0,0, p7pdf1.cov);
227  x_cov.insertMatrix(7,7, p7pdf2.cov);
228 
229  double DUMMY=0;
230  CArrayDouble<2*7> x_incrs;
231  x_incrs.assign(1e-6);
232  transform_gaussian_linear(x_mean,x_cov,func_inv_compose,DUMMY, y_mean,y_cov, x_incrs );
233 
234  }
235  // Compare:
236  EXPECT_NEAR(0, (y_cov-p7_comp.cov).array().abs().mean(), 1e-2 )
237  << "p1 mean: " << p7pdf1.mean << endl
238  << "p2 mean: " << p7pdf2.mean << endl
239  << "Numeric approximation of covariance: " << endl << y_cov << endl
240  << "Returned covariance: " << endl << p7_comp.cov << endl;
241 
242  }
243 
244  void testChangeCoordsRef(
245  double x,double y, double z, double yaw, double pitch, double roll, double std_scale,
246  double x2,double y2, double z2, double yaw2, double pitch2, double roll2 )
247  {
248  CPose3DQuatPDFGaussian p7pdf1 = generateRandomPoseQuat3DPDF(x,y,z,yaw,pitch,roll, std_scale);
249 
250  const CPose3DQuat new_base = CPose3DQuat( CPose3D(x2,y2,z2,yaw2,pitch2,roll2) );
251  const CPose3DQuatPDFGaussian new_base_pdf( new_base, CMatrixDouble77() ); // COV = Zeros
252 
253  const CPose3DQuatPDFGaussian p7_new_base_pdf = new_base_pdf + p7pdf1;
254  p7pdf1.changeCoordinatesReference(new_base);
255 
256  // Compare:
257  EXPECT_NEAR(0, (p7_new_base_pdf.cov - p7pdf1.cov).array().abs().mean(), 1e-2 )
258  << "p1 mean: " << p7pdf1.mean << endl
259  << "new_base: " << new_base << endl;
260  EXPECT_NEAR(0, (p7_new_base_pdf.mean.getAsVectorVal() - p7pdf1.mean.getAsVectorVal() ).array().abs().mean(), 1e-2 )
261  << "p1 mean: " << p7pdf1.mean << endl
262  << "new_base: " << new_base << endl;
263  }
264 
265 };
266 
267 
268 TEST_F(Pose3DQuatPDFGaussTests,ToYPRGaussPDFAndBack)
269 {
270  test_toFromYPRGauss(DEG2RAD(-30),DEG2RAD(10),DEG2RAD(60));
271  test_toFromYPRGauss(DEG2RAD(30),DEG2RAD(88),DEG2RAD(0));
272  test_toFromYPRGauss(DEG2RAD(30),DEG2RAD(89.5),DEG2RAD(0));
273  // The formulas break at pitch=90, but this we cannot avoid...
274 }
275 
276 TEST_F(Pose3DQuatPDFGaussTests,CompositionJacobian)
277 {
278  testCompositionJacobian(0,0,0,DEG2RAD(2),DEG2RAD(0),DEG2RAD(0), 0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
279  testCompositionJacobian(1,2,3,DEG2RAD(2),DEG2RAD(0),DEG2RAD(0), -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
280  testCompositionJacobian(1,-2,3,DEG2RAD(2),DEG2RAD(0),DEG2RAD(0), -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
281  testCompositionJacobian(1,2,-3,DEG2RAD(2),DEG2RAD(0),DEG2RAD(0), -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
282  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
283  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(-80),DEG2RAD(70), -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
284  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(-70), -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
285  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), -8,45,10,DEG2RAD(-50),DEG2RAD(-10),DEG2RAD(30) );
286  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), -8,45,10,DEG2RAD(50),DEG2RAD(10),DEG2RAD(30) );
287  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(-30) );
288 }
289 
290 TEST_F(Pose3DQuatPDFGaussTests,Inverse)
291 {
292  testInverse(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
293  testInverse(0,0,0,DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 0.1 );
294  testInverse(0,0,0,DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 0.1 );
295  testInverse(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 0.1 );
296 
297  testInverse(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
298  testInverse(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.2 );
299 
300  testInverse(1,2,3,DEG2RAD(30),DEG2RAD(0),DEG2RAD(0), 0.1 );
301  testInverse(-1,2,3,DEG2RAD(30),DEG2RAD(0),DEG2RAD(0), 0.1 );
302  testInverse(1,2,-3,DEG2RAD(30),DEG2RAD(0),DEG2RAD(0), 0.1 );
303  testInverse(-1,2,-3,DEG2RAD(30),DEG2RAD(0),DEG2RAD(0), 0.1 );
304  testInverse(1,2,3,DEG2RAD(-30),DEG2RAD(0),DEG2RAD(0), 0.1 );
305  testInverse(-1,2,3,DEG2RAD(-30),DEG2RAD(0),DEG2RAD(0), 0.1 );
306  testInverse(1,2,-3,DEG2RAD(-30),DEG2RAD(0),DEG2RAD(0), 0.1 );
307  testInverse(-1,2,-3,DEG2RAD(-30),DEG2RAD(0),DEG2RAD(0), 0.1 );
308  testInverse(1,2,3,DEG2RAD(0),DEG2RAD(30),DEG2RAD(0), 0.1 );
309  testInverse(-1,2,3,DEG2RAD(0),DEG2RAD(30),DEG2RAD(0), 0.1 );
310  testInverse(1,2,-3,DEG2RAD(0),DEG2RAD(30),DEG2RAD(0), 0.1 );
311  testInverse(-1,2,-3,DEG2RAD(0),DEG2RAD(30),DEG2RAD(0), 0.1 );
312  testInverse(1,2,3,DEG2RAD(0),DEG2RAD(-30),DEG2RAD(0), 0.1 );
313  testInverse(-1,2,3,DEG2RAD(0),DEG2RAD(-30),DEG2RAD(0), 0.1 );
314  testInverse(1,2,-3,DEG2RAD(0),DEG2RAD(-30),DEG2RAD(0), 0.1 );
315  testInverse(-1,2,-3,DEG2RAD(0),DEG2RAD(-30),DEG2RAD(0), 0.1 );
316  testInverse(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(30), 0.1 );
317  testInverse(-1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(30), 0.1 );
318  testInverse(1,2,-3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(30), 0.1 );
319  testInverse(-1,2,-3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(30), 0.1 );
320  testInverse(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(-30), 0.1 );
321  testInverse(-1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(-30), 0.1 );
322  testInverse(1,2,-3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(-30), 0.1 );
323  testInverse(-1,2,-3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(-30), 0.1 );
324 }
325 
326 TEST_F(Pose3DQuatPDFGaussTests,Composition)
327 {
328  testPoseComposition(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, 0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
329  testPoseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
330 
331  testPoseComposition(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.1, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30), 0.1 );
332  testPoseComposition(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.2, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30), 0.2 );
333 
334  testPoseComposition(1,2,3,DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
335  testPoseComposition(1,2,3,DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
336  testPoseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
337  testPoseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 0.1 );
338  testPoseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 0.1 );
339  testPoseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 0.1 );
340 }
341 
342 TEST_F(Pose3DQuatPDFGaussTests,InverseComposition)
343 {
344  testPoseInverseComposition(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, 0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
345  testPoseInverseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
346 
347  testPoseInverseComposition(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.1, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30), 0.1 );
348  testPoseInverseComposition(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.2, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30), 0.2 );
349 
350  testPoseInverseComposition(1,2,3,DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
351  testPoseInverseComposition(1,2,3,DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
352  testPoseInverseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
353  testPoseInverseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 0.1 );
354  testPoseInverseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 0.1 );
355  testPoseInverseComposition(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 0.1 );
356 }
357 
358 
359 TEST_F(Pose3DQuatPDFGaussTests,ChangeCoordsRef)
360 {
361  testChangeCoordsRef(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, 0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
362  testChangeCoordsRef(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
363 
364  testChangeCoordsRef(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.1, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
365  testChangeCoordsRef(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.2, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
366 }
mrpt::poses::CPose3DQuatPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
Definition: CPose3DQuatPDFGaussian.cpp:263
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
z
GLdouble GLdouble z
Definition: glext.h:3734
Pose3DQuatPDFGaussTests::generateRandomPose3DPDF
static CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:39
mrpt::poses::CPoseOrPoint::getAsVectorVal
mrpt::math::CVectorDouble getAsVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:181
Pose3DQuatPDFGaussTests::test_toFromYPRGauss
void test_toFromYPRGauss(double yaw, double pitch, double roll)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:51
TEST_F
TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:268
mrpt::random::randomGenerator
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:23
mrpt::random::CRandomGenerator::drawGaussian1DMatrix
void drawGaussian1DMatrix(MAT &matrix, const double mean=0, const double std=1)
Fills the given matrix with independent, 1D-normally distributed samples.
Definition: RandomGenerators.h:148
mrpt::math::CQuaternionDouble
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
Definition: CQuaternion.h:408
mrpt::poses::CPose3DQuatPDFGaussian::cov
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
Definition: CPose3DQuatPDFGaussian.h:70
std
STL namespace.
CPose3D.h
mrpt::poses::CPose3DQuatPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
Definition: CPose3DQuatPDFGaussian.h:39
Pose3DQuatPDFGaussTests::func_compose
static void func_compose(const CArrayDouble< 2 *7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:66
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:34
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::math::CMatrixDouble77
CMatrixFixedNumeric< double, 7, 7 > CMatrixDouble77
Definition: eigen_frwds.h:51
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
mrpt::math::cov
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:135
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:167
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
mrpt::math::transform_gaussian_linear
void transform_gaussian_linear(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const VECTORLIKE1 &x_increments)
First order uncertainty propagation estimator of the Gaussian distribution of a variable Y=f(X) for a...
Definition: transform_gaussian.h:127
DEG2RAD
#define DEG2RAD
Definition: maps/CColouredPointsMap.h:99
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
Pose3DQuatPDFGaussTests::testInverse
void testInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:176
CPose3DPDFGaussian.h
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:77
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
transform_gaussian.h
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Pose3DQuatPDFGaussTests::testChangeCoordsRef
void testChangeCoordsRef(double x, double y, double z, double yaw, double pitch, double roll, double std_scale, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:244
mrpt::poses::CPose3DQuatPDF::jacobiansPoseComposition
static void jacobiansPoseComposition(const CPose3DQuat &x, const CPose3DQuat &u, mrpt::math::CMatrixDouble77 &df_dx, mrpt::math::CMatrixDouble77 &df_du, CPose3DQuat *out_x_oplus_u=NULL)
This static method computes the two Jacobians of a pose composition operation $f(x,u)= x u$.
Definition: CPose3DQuatPDF.cpp:42
mrpt::math::CArrayDouble
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:37
y
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::math::jacobians::jacob_numeric_estimate
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
Pose3DQuatPDFGaussTests::func_inverse
static void func_inverse(const CArrayDouble< 7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:120
Pose3DQuatPDFGaussTests::testCompositionJacobian
void testCompositionJacobian(double x, double y, double z, double yaw, double pitch, double roll, double x2, double y2, double z2, double yaw2, double pitch2, double roll2)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:128
x
GLenum GLint x
Definition: glext.h:3516
Pose3DQuatPDFGaussTests::testPoseInverseComposition
void testPoseInverseComposition(double x, double y, double z, double yaw, double pitch, double roll, double std_scale, double x2, double y2, double z2, double yaw2, double pitch2, double roll2, double std_scale2)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:208
Pose3DQuatPDFGaussTests::func_inv_compose
static void func_inv_compose(const CArrayDouble< 2 *7 > &x, const double &dummy, CArrayDouble< 7 > &Y)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:75
p
GLfloat GLfloat p
Definition: glext.h:5587
Pose3DQuatPDFGaussTests::testPoseComposition
void testPoseComposition(double x, double y, double z, double yaw, double pitch, double roll, double std_scale, double x2, double y2, double z2, double yaw2, double pitch2, double roll2, double std_scale2)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:84
CPose3DQuatPDFGaussian.h
Pose3DQuatPDFGaussTests::generateRandomPoseQuat3DPDF
static CPose3DQuatPDFGaussian generateRandomPoseQuat3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DQuatPDFGaussian_unittest.cpp:34


Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at mié abr 15 19:30:12 CEST 2020