Main MRPT website > C++ reference for MRPT 1.5.7
CPose3DPDFGaussian_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/CPose3DPDFGaussian.h>
12 #include <mrpt/poses/CPose3DQuatPDFGaussian.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 Pose3DPDFGaussTests : public ::testing::Test {
27 protected:
28  virtual void SetUp()
29  {
30  }
31 
32  virtual void TearDown() { }
33 
34 
35  static CPose3DPDFGaussian generateRandomPose3DPDF(double x,double y, double z, double yaw, double pitch, double roll, double std_scale)
36  {
37  CMatrixDouble61 r;
38  mrpt::random::randomGenerator.drawGaussian1DMatrix(r, 0,std_scale);
39  CMatrixDouble66 cov;
40  cov.multiply_AAt(r); // random semi-definite positive matrix:
41  for (int i=0;i<6;i++) cov(i,i)+=1e-7;
42  CPose3DPDFGaussian p6pdf( CPose3D(x,y,z,yaw,pitch,roll), cov );
43  return p6pdf;
44  }
45 
46 
47  void testToQuatPDFAndBack(double x,double y, double z, double yaw, double pitch, double roll, double std_scale)
48  {
49  CPose3DPDFGaussian p6pdf = generateRandomPose3DPDF(x,y,z,yaw,pitch,roll, std_scale);
50  //cout << "p6pdf: " << p6pdf << endl;
51  CPose3DQuatPDFGaussian p7pdf = CPose3DQuatPDFGaussian(p6pdf);
52  //cout << "p7pdf: " << p7pdf << endl;
53  CPose3DPDFGaussian p6pdf_recov = CPose3DPDFGaussian(p7pdf);
54  //cout << "p6pdf_recov: " << p6pdf_recov << endl;
55 
56  const double val_mean_error = (p6pdf_recov.mean.getAsVectorVal() - p6pdf.mean.getAsVectorVal()).array().abs().mean();
57  const double cov_mean_error = (p6pdf_recov.cov - p6pdf.cov).array().abs().mean();
58  //cout << "cov err: " << cov_mean_error << " " << "val_mean_error: " << val_mean_error << endl;
59  EXPECT_TRUE(val_mean_error < 1e-8);
60  EXPECT_TRUE(cov_mean_error < 1e-8);
61  }
62 
63 
64  static void func_compose(const CArrayDouble<12> &x, const double &dummy, CArrayDouble<6> &Y)
65  {
66  MRPT_UNUSED_PARAM(dummy);
67  const CPose3D p1(x[0],x[1],x[2],x[3],x[4],x[5]);
68  const CPose3D p2(x[6+0],x[6+1],x[6+2],x[6+3],x[6+4],x[6+5]);
69  const CPose3D p = p1+p2;
70  for (int i=0;i<6;i++) Y[i]=p[i];
71  }
72 
73  static void func_inv_compose(const CArrayDouble<2*6> &x, const double &dummy, CArrayDouble<6> &Y)
74  {
75  MRPT_UNUSED_PARAM(dummy);
76  const CPose3D p1(x[0],x[1],x[2],x[3],x[4],x[5]);
77  const CPose3D p2(x[6+0],x[6+1],x[6+2],x[6+3],x[6+4],x[6+5]);
78  const CPose3D p = p1-p2;
79  for (int i=0;i<6;i++) Y[i]=p[i];
80  }
81 
82  // Test "+" & "+=" operator
83  void testPoseComposition(
84  double x,double y, double z, double yaw, double pitch, double roll, double std_scale,
85  double x2,double y2, double z2, double yaw2, double pitch2, double roll2, double std_scale2 )
86  {
87  CPose3DPDFGaussian p6pdf1 = generateRandomPose3DPDF(x,y,z,yaw,pitch,roll, std_scale);
88  CPose3DPDFGaussian p6pdf2 = generateRandomPose3DPDF(x2,y2,z2,yaw2,pitch2,roll2, std_scale2);
89 
90  // With "+" operators:
91  CPose3DPDFGaussian p6_comp = p6pdf1 + p6pdf2;
92 
93  // Numeric approximation:
94  CArrayDouble<6> y_mean;
95  CMatrixFixedNumeric<double,6,6> y_cov;
96  {
97  CArrayDouble<12> x_mean;
98  for (int i=0;i<6;i++) x_mean[i]=p6pdf1.mean[i];
99  for (int i=0;i<6;i++) x_mean[6+i]=p6pdf2.mean[i];
100 
101  CMatrixFixedNumeric<double,12,12> x_cov;
102  x_cov.insertMatrix(0,0, p6pdf1.cov);
103  x_cov.insertMatrix(6,6, p6pdf2.cov);
104 
105  double DUMMY=0;
106  CArrayDouble<12> 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  // Compare mean:
111  EXPECT_NEAR(0, (y_mean-p6_comp.mean.getAsVectorVal()).array().abs().sum(), 1e-2 )
112  << "p1 mean: " << p6pdf1.mean << endl
113  << "p2 mean: " << p6pdf2.mean << endl;
114 
115  // Compare cov:
116  EXPECT_NEAR(0, (y_cov-p6_comp.cov).array().abs().sum(), 1e-2 )
117  << "p1 mean: " << p6pdf1.mean << endl
118  << "p2 mean: " << p6pdf2.mean << endl;
119 
120  // Test +=
121  p6_comp = p6pdf1;
122  p6_comp += p6pdf2;
123 
124  // Compare mean:
125  EXPECT_NEAR(0, (y_mean-p6_comp.mean.getAsVectorVal()).array().abs().sum(), 1e-2 )
126  << "p1 mean: " << p6pdf1.mean << endl
127  << "p2 mean: " << p6pdf2.mean << endl;
128 
129  // Compare cov:
130  EXPECT_NEAR(0, (y_cov-p6_comp.cov).array().abs().sum(), 1e-2 )
131  << "p1 mean: " << p6pdf1.mean << endl
132  << "p2 mean: " << p6pdf2.mean << endl;
133  }
134 
135 
136  void testCompositionJacobian(
137  double x,double y, double z, double yaw, double pitch, double roll,
138  double x2,double y2, double z2, double yaw2, double pitch2, double roll2)
139  {
140  const CPose3D q1(x,y,z,yaw,pitch,roll);
141  const CPose3D q2(x2,y2,z2,yaw2,pitch2,roll2);
142 
143  // Theoretical Jacobians:
144  CMatrixDouble66 df_dx(UNINITIALIZED_MATRIX), df_du(UNINITIALIZED_MATRIX);
145  CPose3DPDF::jacobiansPoseComposition(
146  q1, // x
147  q2, // u
148  df_dx,
149  df_du );
150 
151  // Numerical approximation:
152  CMatrixDouble66 num_df_dx(UNINITIALIZED_MATRIX), num_df_du(UNINITIALIZED_MATRIX);
153  {
154  CArrayDouble<2*6> x_mean;
155  for (int i=0;i<6;i++) x_mean[i]=q1[i];
156  for (int i=0;i<6;i++) x_mean[6+i]=q2[i];
157 
158  double DUMMY=0;
159  CArrayDouble<2*6> x_incrs;
160  x_incrs.assign(1e-7);
161  CMatrixDouble numJacobs;
162  mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose,x_incrs, DUMMY, numJacobs );
163 
164  numJacobs.extractMatrix(0,0, num_df_dx);
165  numJacobs.extractMatrix(0,6, num_df_du);
166  }
167 
168  // Compare:
169  EXPECT_NEAR(0, (df_dx-num_df_dx).array().abs().sum(), 3e-3 )
170  << "q1: " << q1 << endl
171  << "q2: " << q2 << endl
172  << "Numeric approximation of df_dx: " << endl << num_df_dx << endl
173  << "Implemented method: " << endl << df_dx << endl
174  << "Error: " << endl << df_dx-num_df_dx << endl;
175 
176  EXPECT_NEAR(0, (df_du-num_df_du).array().abs().sum(), 3e-3 )
177  << "q1: " << q1 << endl
178  << "q2: " << q2 << endl
179  << "Numeric approximation of df_du: " << endl << num_df_du << endl
180  << "Implemented method: " << endl << df_du << endl
181  << "Error: " << endl << df_du-num_df_du << endl;
182 
183  }
184 
185  // Test the "-" & "-=" operator
186  void testPoseInverseComposition(
187  double x,double y, double z, double yaw, double pitch, double roll, double std_scale,
188  double x2,double y2, double z2, double yaw2, double pitch2, double roll2, double std_scale2 )
189  {
190  CPose3DPDFGaussian p6pdf1 = generateRandomPose3DPDF(x,y,z,yaw,pitch,roll, std_scale);
191  CPose3DPDFGaussian p6pdf2 = generateRandomPose3DPDF(x2,y2,z2,yaw2,pitch2,roll2, std_scale2);
192 
193  // With the "-" operator
194  CPose3DPDFGaussian p6_comp = p6pdf1 - p6pdf2;
195 
196  // Numeric approximation:
197  CArrayDouble<6> y_mean;
198  CMatrixFixedNumeric<double,6,6> y_cov;
199  {
200  CArrayDouble<2*6> x_mean;
201  for (int i=0;i<6;i++) x_mean[i]=p6pdf1.mean[i];
202  for (int i=0;i<6;i++) x_mean[6+i]=p6pdf2.mean[i];
203 
204  CMatrixFixedNumeric<double,12,12> x_cov;
205  x_cov.insertMatrix(0,0, p6pdf1.cov);
206  x_cov.insertMatrix(6,6, p6pdf2.cov);
207 
208  double DUMMY=0;
209  CArrayDouble<2*6> x_incrs;
210  x_incrs.assign(1e-6);
211  transform_gaussian_linear(x_mean,x_cov,func_inv_compose,DUMMY, y_mean,y_cov, x_incrs );
212  }
213  // Compare mean:
214  EXPECT_NEAR(0, (y_mean-p6_comp.mean.getAsVectorVal()).array().abs().sum(), 1e-2 )
215  << "p1 mean: " << p6pdf1.mean << endl
216  << "p2 mean: " << p6pdf2.mean << endl;
217 
218  // Compare cov:
219  EXPECT_NEAR(0, (y_cov-p6_comp.cov).array().abs().sum(), 1e-2 )
220  << "p1 mean: " << p6pdf1.mean << endl
221  << "p2 mean: " << p6pdf2.mean << endl;
222 
223  // With the "-=" operator
224  p6_comp = p6pdf1;
225  p6_comp -= p6pdf2;
226 
227  // Compare mean:
228  EXPECT_NEAR(0, (y_mean-p6_comp.mean.getAsVectorVal()).array().abs().sum(), 1e-2 )
229  << "p1 mean: " << p6pdf1.mean << endl
230  << "p2 mean: " << p6pdf2.mean << endl;
231 
232  // Compare cov:
233  EXPECT_NEAR(0, (y_cov-p6_comp.cov).array().abs().sum(), 1e-2 )
234  << "p1 mean: " << p6pdf1.mean << endl
235  << "p2 mean: " << p6pdf2.mean << endl;
236  }
237 
238  // Test the unary "-" operator
239  void testPoseInverse(
240  double x,double y, double z, double yaw, double pitch, double roll, double std_scale )
241  {
242  CPose3DPDFGaussian p6pdf2 = generateRandomPose3DPDF(x,y,z,yaw,pitch,roll, std_scale);
243  CPose3DPDFGaussian p6_zero( CPose3D(0,0,0, 0,0,0), CMatrixDouble66() ); // COV=All zeros
244 
245  // Unary "-":
246  const CPose3DPDFGaussian p6_inv = -p6pdf2;
247 
248  // Compare to the binary "-" operator:
249  const CPose3DPDFGaussian p6_comp = p6_zero - p6pdf2;
250 
251  // Compare mean:
252  EXPECT_NEAR(0, (p6_inv.mean.getAsVectorVal()-p6_comp.mean.getAsVectorVal()).array().abs().sum(), 1e-2 )
253  << "p mean: " << p6pdf2.mean << endl;
254 
255  // Compare cov:
256  EXPECT_NEAR(0, (p6_inv.cov-p6_comp.cov).array().abs().sum(), 1e-2 )
257  << "p mean: " << p6pdf2.mean << endl;
258 
259 
260  // Compare to the "inverse()" method:
261  CPose3DPDFGaussian p6_inv2;
262  p6pdf2.inverse(p6_inv2);
263 
264  // Compare mean:
265  EXPECT_NEAR(0, (p6_inv2.mean.getAsVectorVal()-p6_comp.mean.getAsVectorVal()).array().abs().sum(), 1e-2 )
266  << "p mean: " << p6pdf2.mean << endl
267  << "p6_inv2 mean: " << p6_inv2.mean << endl
268  << "p6_comp mean: " << p6_comp.mean << endl;
269 
270  // Compare cov:
271  EXPECT_NEAR(0, (p6_inv2.cov-p6_comp.cov).array().abs().sum(), 1e-2 )
272  << "p mean: " << p6pdf2.mean << endl
273  << "p6_inv2 mean: " << p6_inv2.mean << endl
274  << "p6_comp mean: " << p6_comp.mean << endl;
275  }
276 
277  // Test all operators
278  void testAllPoseOperators(
279  double x,double y, double z, double yaw, double pitch, double roll, double std_scale,
280  double x2,double y2, double z2, double yaw2, double pitch2, double roll2, double std_scale2 )
281  {
282  // +, +=
283  testPoseComposition(x,y,z,yaw,pitch,roll,std_scale, x2,y2,z2,yaw2,pitch2,roll2,std_scale2 );
284  // -, -=, unary "-"
285  testPoseInverseComposition(x,y,z,yaw,pitch,roll,std_scale, x2,y2,z2,yaw2,pitch2,roll2,std_scale2 );
286  // unitary "-" & ".inverse()"
287  testPoseInverse(x,y,z,yaw,pitch,roll,std_scale);
288  }
289 
290 
291  void testChangeCoordsRef(
292  double x,double y, double z, double yaw, double pitch, double roll, double std_scale,
293  double x2,double y2, double z2, double yaw2, double pitch2, double roll2 )
294  {
295  CPose3DPDFGaussian p6pdf1 = generateRandomPose3DPDF(x,y,z,yaw,pitch,roll, std_scale);
296 
297  const CPose3D new_base = CPose3D(x2,y2,z2,yaw2,pitch2,roll2);
298  const CPose3DPDFGaussian new_base_pdf( new_base, CMatrixDouble66() ); // COV = Zeros
299 
300  const CPose3DPDFGaussian p6_new_base_pdf = new_base_pdf + p6pdf1;
301  p6pdf1.changeCoordinatesReference(new_base);
302 
303 
304  // Compare:
305  EXPECT_NEAR(0, (p6_new_base_pdf.cov - p6pdf1.cov).array().abs().mean(), 1e-2 )
306  << "p1 mean: " << p6pdf1.mean << endl
307  << "new_base: " << new_base << endl;
308  EXPECT_NEAR(0, (p6_new_base_pdf.mean.getAsVectorVal() - p6pdf1.mean.getAsVectorVal() ).array().abs().mean(), 1e-2 )
309  << "p1 mean: " << p6pdf1.mean << endl
310  << "new_base: " << new_base << endl;
311  }
312 
313 };
314 
315 
316 TEST_F(Pose3DPDFGaussTests,ToQuatGaussPDFAndBack)
317 {
318  testToQuatPDFAndBack(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
319  testToQuatPDFAndBack(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.2 );
320 
321  testToQuatPDFAndBack(6,-2,-3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
322  testToQuatPDFAndBack(6,-2,-3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.2 );
323 
324  testToQuatPDFAndBack(6,-2,-3,DEG2RAD(10),DEG2RAD(40),DEG2RAD(5), 0.1 );
325  testToQuatPDFAndBack(6,-2,-3,DEG2RAD(10),DEG2RAD(40),DEG2RAD(5), 0.2 );
326 
327  testToQuatPDFAndBack(6,-2,-3,DEG2RAD(-50),DEG2RAD(87),DEG2RAD(20), 0.1 );
328  testToQuatPDFAndBack(6,-2,-3,DEG2RAD(-50),DEG2RAD(87),DEG2RAD(20), 0.2 );
329 
330  testToQuatPDFAndBack(6,-2,-3,DEG2RAD(-50),DEG2RAD(-87),DEG2RAD(20), 0.1 );
331  testToQuatPDFAndBack(6,-2,-3,DEG2RAD(-50),DEG2RAD(-87),DEG2RAD(20), 0.2 );
332 }
333 
334 TEST_F(Pose3DPDFGaussTests,CompositionJacobian)
335 {
336  testCompositionJacobian(0,0,0,DEG2RAD(2),DEG2RAD(0),DEG2RAD(0), 0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
337  testCompositionJacobian(0,0,0,DEG2RAD(2),DEG2RAD(0),DEG2RAD(0), 0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
338  testCompositionJacobian(1,2,3,DEG2RAD(2),DEG2RAD(0),DEG2RAD(0), -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
339  testCompositionJacobian(1,-2,3,DEG2RAD(2),DEG2RAD(0),DEG2RAD(0), -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
340  testCompositionJacobian(1,2,-3,DEG2RAD(2),DEG2RAD(0),DEG2RAD(0), -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
341  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
342  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(-80),DEG2RAD(70), -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
343  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(-70), -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
344  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), -8,45,10,DEG2RAD(-50),DEG2RAD(-10),DEG2RAD(30) );
345  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), -8,45,10,DEG2RAD(50),DEG2RAD(10),DEG2RAD(30) );
346  testCompositionJacobian(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(-30) );
347 }
348 
349 // Test the +, -, +=, -=, "-" operators
350 TEST_F(Pose3DPDFGaussTests,AllOperators)
351 {
352  testAllPoseOperators(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, 0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
353  testAllPoseOperators(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
354 
355  testAllPoseOperators(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.1, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30), 0.1 );
356  testAllPoseOperators(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.2, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30), 0.2 );
357 
358  testAllPoseOperators(1,2,3,DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
359  testAllPoseOperators(1,2,3,DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
360  testAllPoseOperators(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1 );
361  testAllPoseOperators(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(10),DEG2RAD(0),DEG2RAD(0), 0.1 );
362  testAllPoseOperators(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(10),DEG2RAD(0), 0.1 );
363  testAllPoseOperators(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(10), 0.1 );
364 }
365 
366 TEST_F(Pose3DPDFGaussTests,ChangeCoordsRef)
367 {
368  testChangeCoordsRef(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, 0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
369  testChangeCoordsRef(1,2,3,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0), 0.1, -8,45,10,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
370 
371  testChangeCoordsRef(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.1, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
372  testChangeCoordsRef(1,2,3,DEG2RAD(20),DEG2RAD(80),DEG2RAD(70), 0.2, -8,45,10,DEG2RAD(50),DEG2RAD(-10),DEG2RAD(30) );
373 }
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:73
z
GLdouble GLdouble z
Definition: glext.h:3734
Pose3DPDFGaussTests::testAllPoseOperators
void testAllPoseOperators(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: CPose3DPDFGaussian_unittest.cpp:278
Pose3DPDFGaussTests::func_compose
static void func_compose(const CArrayDouble< 12 > &x, const double &dummy, CArrayDouble< 6 > &Y)
Definition: CPose3DPDFGaussian_unittest.cpp:64
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
mrpt::poses::CPose3DPDF::jacobiansPoseComposition
static void jacobiansPoseComposition(const CPose3D &x, const CPose3D &u, mrpt::math::CMatrixDouble66 &df_dx, mrpt::math::CMatrixDouble66 &df_du)
This static method computes the pose composition Jacobians.
Definition: CPose3DPDF.cpp:133
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
TEST_F
TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
Definition: CPose3DPDFGaussian_unittest.cpp:316
std
STL namespace.
CPose3D.h
Pose3DPDFGaussTests::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: CPose3DPDFGaussian_unittest.cpp:136
Pose3DPDFGaussTests::testPoseInverse
void testPoseInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DPDFGaussian_unittest.cpp:239
mrpt::poses::CPose3DQuatPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
Definition: CPose3DQuatPDFGaussian.h:39
Pose3DPDFGaussTests::func_inv_compose
static void func_inv_compose(const CArrayDouble< 2 *6 > &x, const double &dummy, CArrayDouble< 6 > &Y)
Definition: CPose3DPDFGaussian_unittest.cpp:73
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_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::CPose3DPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
Definition: CPose3DPDFGaussian.cpp:332
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
CPose3DPDFGaussian.h
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:77
Pose3DPDFGaussTests::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: CPose3DPDFGaussian_unittest.cpp:291
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
Pose3DPDFGaussTests::generateRandomPose3DPDF
static CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DPDFGaussian_unittest.cpp:35
Pose3DPDFGaussTests::testToQuatPDFAndBack
void testToQuatPDFAndBack(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Definition: CPose3DPDFGaussian_unittest.cpp:47
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
Pose3DPDFGaussTests::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: CPose3DPDFGaussian_unittest.cpp:186
x
GLenum GLint x
Definition: glext.h:3516
mrpt::math::CMatrixDouble66
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
Definition: eigen_frwds.h:50
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::poses::CPose3DPDFGaussian::inverse
void inverse(CPose3DPDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Definition: CPose3DPDFGaussian.cpp:450
Pose3DPDFGaussTests::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: CPose3DPDFGaussian_unittest.cpp:83
CPose3DQuatPDFGaussian.h


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019