Main MRPT website > C++ reference for MRPT 1.5.7
CPosePDFGaussian_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 
11 #include <mrpt/random.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 
23 class PosePDFGaussTests : public ::testing::Test {
24 protected:
25  virtual void SetUp()
26  {
27  }
28 
29  virtual void TearDown() { }
30 
31  static CPosePDFGaussian generateRandomPose2DPDF(double x,double y, double phi, double std_scale)
32  {
36  cov.multiply_AAt(r); // random semi-definite positive matrix:
37  for (int i=0;i<3;i++) cov(i,i)+=1e-7;
38  CPosePDFGaussian pdf( CPose2D(x,y,phi), cov );
39  return pdf;
40  }
41 
42  static void func_inverse(const CArrayDouble<3> &x, const double &dummy, CArrayDouble<3> &Y)
43  {
44  MRPT_UNUSED_PARAM(dummy);
45  const CPose2D p1(x[0],x[1],x[2]);
46  const CPose2D p1_inv = CPose2D() - p1;
47  for (int i=0;i<3;i++) Y[i]=p1_inv[i];
48  }
49 
51  double x,double y, double phi, double std_scale)
52  {
53  CPosePDFGaussian pdf1 = generateRandomPose2DPDF(x,y,phi, std_scale);
54 
55  CPosePDFGaussian pdf1_inv;
56  pdf1.inverse(pdf1_inv);
57 
58  // Numeric approximation:
59  CArrayDouble<3> y_mean;
61  {
62  CArrayDouble<3> x_mean;
63  for (int i=0;i<3;i++) x_mean[i]=pdf1.mean[i];
64 
66 
67  double DUMMY=0;
68  CArrayDouble<3> x_incrs;
69  x_incrs.assign(1e-6);
70  transform_gaussian_linear(x_mean,x_cov,func_inverse,DUMMY, y_mean,y_cov, x_incrs );
71  }
72  // Compare:
73  EXPECT_NEAR(0, (y_cov-pdf1_inv.cov).array().abs().mean(), 1e-5 )
74  << "pdf1 mean: " << pdf1.mean << endl
75  << "Numeric approximation of covariance: " << endl << y_cov << endl
76  << "Returned covariance: " << endl << pdf1_inv.cov << endl;
77  }
78 
79 
80 };
81 
83 {
84  testPoseInverse(0,0,0, 0.01);
85  testPoseInverse(0,0,0, 0.1);
86 
87  testPoseInverse(1,0,0, 0.1);
88  testPoseInverse(0,1,0, 0.1);
89  testPoseInverse(0,0,1, 0.1);
90 
91  testPoseInverse(-5,0,0, 0.1);
92  testPoseInverse(0,-5,0, 0.1);
93  testPoseInverse(0,0,-5, 0.1);
94 
95  testPoseInverse(4,6,DEG2RAD(10), 0.1);
96  testPoseInverse(4,6,DEG2RAD(-10), 0.1);
97 
98  testPoseInverse(-7,2,DEG2RAD(30), 0.1);
99  testPoseInverse(-7,2,DEG2RAD(-30), 0.1);
100 }
101 
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
CPose2D mean
The mean value.
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.
STL namespace.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
static void func_inverse(const CArrayDouble< 3 > &x, const double &dummy, CArrayDouble< 3 > &Y)
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#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
void inverse(CPosePDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void testPoseInverse(double x, double y, double phi, double std_scale)
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
static CPosePDFGaussian generateRandomPose2DPDF(double x, double y, double phi, double std_scale)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
TEST_F(PosePDFGaussTests, Inverse)
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 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
GLenum GLint GLint y
Definition: glext.h:3516
GLenum GLint x
Definition: glext.h:3516



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