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 }
mrpt::math::cov
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
mrpt::math::MatrixVectorBase::fill
void fill(const Scalar &val)
Definition: MatrixVectorBase.h:70
mrpt::poses::CPosePDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPosePDFGaussian.h:46
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
PosePDFGaussTests::testPoseInverse
void testPoseInverse(double x, double y, double phi, double std_scale)
Definition: CPosePDFGaussian_unittest.cpp:51
random.h
PosePDFGaussTests::TearDown
void TearDown() override
Definition: CPosePDFGaussian_unittest.cpp:28
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:44
mrpt::math::transform_gaussian_linear
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...
Definition: transform_gaussian.h:152
PosePDFGaussTests::func_inverse
static void func_inverse(const CVectorFixedDouble< 3 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 3 > &Y)
Definition: CPosePDFGaussian_unittest.cpp:42
PosePDFGaussTests::SetUp
void SetUp() override
Definition: CPosePDFGaussian_unittest.cpp:27
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
PosePDFGaussTests::generateRandomPose2DPDF
static CPosePDFGaussian generateRandomPose2DPDF(double x, double y, double phi, double std_scale)
Definition: CPosePDFGaussian_unittest.cpp:29
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::math::MatrixBase::matProductOf_AAt
void matProductOf_AAt(const MAT_A &A)
this = A * AT
Definition: MatrixBase.h:276
PosePDFGaussTests
Definition: CPosePDFGaussian_unittest.cpp:24
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
CPosePDFGaussian.h
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
TEST_F
TEST_F(PosePDFGaussTests, Inverse)
Definition: CPosePDFGaussian_unittest.cpp:83
mrpt::poses::CPosePDFGaussian::inverse
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Definition: CPosePDFGaussian.cpp:297
transform_gaussian.h
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:28
mrpt::random::CRandomGenerator::drawGaussian1DMatrix
void drawGaussian1DMatrix(MAT &matrix, const double mean=0, const double std=1)
Fills the given matrix with independent, 1D-normally distributed samples.
Definition: RandomGenerators.h:206



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020