MRPT  2.0.4
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-2020, 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 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  void SetUp() override {}
28  void TearDown() override {}
30  double x, double y, double phi, double std_scale)
31  {
34  r, 0, std_scale);
36  cov.matProductOf_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 CVectorFixedDouble<3>& x, [[maybe_unused]] const double& dummy,
45  {
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  CVectorFixedDouble<3> y_mean;
61  {
62  CVectorFixedDouble<3> x_mean;
63  for (int i = 0; i < 3; i++) x_mean[i] = pdf1.mean[i];
64 
65  CMatrixFixed<double, 3, 3> x_cov = pdf1.cov;
66 
67  double DUMMY = 0;
68  CVectorFixedDouble<3> x_incrs;
69  x_incrs.fill(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, 10.0_deg, 0.1);
97  testPoseInverse(4, 6, -10.0_deg, 0.1);
98 
99  testPoseInverse(-7, 2, 30.0_deg, 0.1);
100  testPoseInverse(-7, 2, -30.0_deg, 0.1);
101 }
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.
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 ...
static void func_inverse(const CVectorFixedDouble< 3 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 3 > &Y)
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.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
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.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020