Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 <CTraitsTest.h>
14 #include <gtest/gtest.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::poses;
18 
19 using namespace mrpt::math;
20 using namespace std;
21 
22 template class mrpt::CTraitsTest<CPosePDFGaussian>;
23 
24 class PosePDFGaussTests : public ::testing::Test
25 {
26  protected:
27  virtual void SetUp() {}
28  virtual void TearDown() {}
30  double x, double y, double phi, double std_scale)
31  {
34  r, 0, std_scale);
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(
43  const CArrayDouble<3>& x, const double& dummy, CArrayDouble<3>& Y)
44  {
45  MRPT_UNUSED_PARAM(dummy);
46  const CPose2D p1(x[0], x[1], x[2]);
47  const CPose2D p1_inv = CPose2D() - p1;
48  for (int i = 0; i < 3; i++) Y[i] = p1_inv[i];
49  }
50 
51  void testPoseInverse(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);
71  x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
72  }
73  // Compare:
74  EXPECT_NEAR(0, (y_cov - pdf1_inv.cov).array().abs().mean(), 1e-5)
75  << "pdf1 mean: " << pdf1.mean << endl
76  << "Numeric approximation of covariance: " << endl
77  << y_cov << endl
78  << "Returned covariance: " << endl
79  << pdf1_inv.cov << endl;
80  }
81 };
82 
84 {
85  testPoseInverse(0, 0, 0, 0.01);
86  testPoseInverse(0, 0, 0, 0.1);
87 
88  testPoseInverse(1, 0, 0, 0.1);
89  testPoseInverse(0, 1, 0, 0.1);
90  testPoseInverse(0, 0, 1, 0.1);
91 
92  testPoseInverse(-5, 0, 0, 0.1);
93  testPoseInverse(0, -5, 0, 0.1);
94  testPoseInverse(0, 0, -5, 0.1);
95 
96  testPoseInverse(4, 6, DEG2RAD(10), 0.1);
97  testPoseInverse(4, 6, DEG2RAD(-10), 0.1);
98 
99  testPoseInverse(-7, 2, DEG2RAD(30), 0.1);
100  testPoseInverse(-7, 2, DEG2RAD(-30), 0.1);
101 }
CPose2D mean
The mean value.
double DEG2RAD(const double x)
Degrees to radians.
void drawGaussian1DMatrix(MAT &matrix, const double mean=0, const double std=1)
Fills the given matrix with independent, 1D-normally distributed samples.
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
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.
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 ...
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...
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...
TEST_F(PosePDFGaussTests, Inverse)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
GLenum GLint GLint y
Definition: glext.h:3538
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:148
GLenum GLint x
Definition: glext.h:3538
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019