MRPT  1.9.9
CPosePDFGaussian_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <CTraitsTest.h>
11 #include <gtest/gtest.h>
14 #include <mrpt/random.h>
15 #include <Eigen/Dense>
16 
17 using namespace mrpt;
18 using namespace mrpt::poses;
19 
20 using namespace mrpt::math;
21 using namespace std;
22 
23 template class mrpt::CTraitsTest<CPosePDFGaussian>;
24 
25 class PosePDFGaussTests : public ::testing::Test
26 {
27  protected:
28  void SetUp() override {}
29  void TearDown() override {}
31  double x, double y, double phi, double std_scale)
32  {
35  r, 0, std_scale);
37  cov.matProductOf_AAt(r); // random semi-definite positive matrix:
38  for (int i = 0; i < 3; i++) cov(i, i) += 1e-7;
39  CPosePDFGaussian pdf(CPose2D(x, y, phi), cov);
40  return pdf;
41  }
42 
43  static void func_inverse(
44  const CVectorFixedDouble<3>& x, const double& dummy,
46  {
47  MRPT_UNUSED_PARAM(dummy);
48  const CPose2D p1(x[0], x[1], x[2]);
49  const CPose2D p1_inv = CPose2D() - p1;
50  for (int i = 0; i < 3; i++) Y[i] = p1_inv[i];
51  }
52 
53  void testPoseInverse(double x, double y, double phi, double std_scale)
54  {
55  CPosePDFGaussian pdf1 = generateRandomPose2DPDF(x, y, phi, std_scale);
56 
57  CPosePDFGaussian pdf1_inv;
58  pdf1.inverse(pdf1_inv);
59 
60  // Numeric approximation:
61  CVectorFixedDouble<3> y_mean;
63  {
64  CVectorFixedDouble<3> x_mean;
65  for (int i = 0; i < 3; i++) x_mean[i] = pdf1.mean[i];
66 
67  CMatrixFixed<double, 3, 3> x_cov = pdf1.cov;
68 
69  double DUMMY = 0;
70  CVectorFixedDouble<3> x_incrs;
71  x_incrs.fill(1e-6);
73  x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
74  }
75  // Compare:
76  EXPECT_NEAR(0, (y_cov - pdf1_inv.cov).array().abs().mean(), 1e-5)
77  << "pdf1 mean: " << pdf1.mean << endl
78  << "Numeric approximation of covariance: " << endl
79  << y_cov << endl
80  << "Returned covariance: " << endl
81  << pdf1_inv.cov << endl;
82  }
83 };
84 
86 {
87  testPoseInverse(0, 0, 0, 0.01);
88  testPoseInverse(0, 0, 0, 0.1);
89 
90  testPoseInverse(1, 0, 0, 0.1);
91  testPoseInverse(0, 1, 0, 0.1);
92  testPoseInverse(0, 0, 1, 0.1);
93 
94  testPoseInverse(-5, 0, 0, 0.1);
95  testPoseInverse(0, -5, 0, 0.1);
96  testPoseInverse(0, 0, -5, 0.1);
97 
98  testPoseInverse(4, 6, 10.0_deg, 0.1);
99  testPoseInverse(4, 6, -10.0_deg, 0.1);
100 
101  testPoseInverse(-7, 2, 30.0_deg, 0.1);
102  testPoseInverse(-7, 2, -30.0_deg, 0.1);
103 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
CPose2D mean
The mean value.
void fill(const Scalar &val)
void drawGaussian1DMatrix(MAT &matrix, const double mean=0, const double std=1)
Fills the given matrix with independent, 1D-normally distributed samples.
static void func_inverse(const CVectorFixedDouble< 3 > &x, const double &dummy, CVectorFixedDouble< 3 > &Y)
STL namespace.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
This base provides a set of functions for maths stuff.
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...
CMatrixDouble 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:149
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:3711
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
GLenum GLint GLint y
Definition: glext.h:3542
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
GLenum GLint x
Definition: glext.h:3542
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: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019