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>
13 #include <mrpt/random.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  {
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;
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
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;
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 
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 
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:
146  q1, // x
147  q2, // u
148  df_dx,
149  df_du );
150 
151  // Numerical approximation:
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
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;
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 
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
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
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 
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
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 }
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose3D mean
The mean value.
GLdouble GLdouble z
Definition: glext.h:3734
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)
static void func_compose(const CArrayDouble< 12 > &x, const double &dummy, CArrayDouble< 6 > &Y)
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
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
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
void drawGaussian1DMatrix(MAT &matrix, const double mean=0, const double std=1)
Fills the given matrix with independent, 1D-normally distributed samples.
TEST_F(Pose3DPDFGaussTests, ToQuatGaussPDFAndBack)
STL namespace.
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)
void testPoseInverse(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
static void func_inv_compose(const CArrayDouble< 2 *6 > &x, const double &dummy, CArrayDouble< 6 > &Y)
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
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...
#define DEG2RAD
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
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)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
static CPose3DPDFGaussian generateRandomPose3DPDF(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
void testToQuatPDFAndBack(double x, double y, double z, double yaw, double pitch, double roll, double std_scale)
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
GLenum GLint GLint y
Definition: glext.h:3516
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 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)
GLenum GLint x
Definition: glext.h:3516
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
Definition: eigen_frwds.h:50
GLfloat GLfloat p
Definition: glext.h:5587
void inverse(CPose3DPDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
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)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019