MRPT  2.0.1
CPoint2DPDFGaussian.h
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 #pragma once
10 
11 #include <mrpt/poses/CPoint2DPDF.h>
12 
13 namespace mrpt::poses
14 {
15 /** A gaussian distribution for 2D points. Also a method for bayesian fusion is
16  * provided.
17  * \ingroup poses_pdf_grp
18  * \sa CPoint2DPDF
19  */
21 {
24 
25  public:
26  /** Default constructor */
28  /** Constructor */
29  CPoint2DPDFGaussian(const CPoint2D& init_Mean);
30  /** Constructor */
32  const CPoint2D& init_Mean, const mrpt::math::CMatrixDouble22& init_Cov);
33 
34  /** The mean value */
36  /** The 2x2 covariance matrix */
38 
39  /** Returns an estimate of the point, (the mean, or mathematical expectation
40  * of the PDF) */
41  void getMean(CPoint2D& p) const override { p = this->mean; }
42  /** Returns an estimate of the point covariance matrix (2x2 cov matrix) and
43  * the mean, both at once. \sa getMean */
44  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override
45  {
46  return {this->cov, this->mean};
47  }
48 
49  /** Copy operator, translating if necesary (for example, between particles
50  * and gaussian representations) */
51  void copyFrom(const CPoint2DPDF& o) override;
52 
53  /** Save PDF's particles to a text file, containing the 2D pose in the first
54  * line, then the covariance matrix in next 3 lines */
55  bool saveToTextFile(const std::string& file) const override;
56 
57  /** this = p (+) this. This can be used to convert a PDF from local
58  * coordinates to global, providing the point (newReferenceBase) from which
59  * "to project" the current pdf. Result PDF substituted the currently
60  * stored one in the object. Both the mean value and the covariance matrix
61  * are updated correctly. */
62  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
63 
64  /** Bayesian fusion of two points gauss. distributions, then save the result
65  *in this object.
66  * The process is as follows:<br>
67  * - (x1,S1): Mean and variance of the p1 distribution.
68  * - (x2,S2): Mean and variance of the p2 distribution.
69  * - (x,S): Mean and variance of the resulting distribution.
70  *
71  * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>;
72  * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 );
73  */
74  void bayesianFusion(
75  const CPoint2DPDFGaussian& p1, const CPoint2DPDFGaussian& p2);
76 
77  /** Computes the "correspondence likelihood" of this PDF with another one:
78  * This is implemented as the integral from -inf to +inf of the product of
79  * both PDF.
80  * The resulting number is >=0.
81  * \sa productIntegralNormalizedWith
82  * \exception std::exception On errors like covariance matrix with null
83  * determinant, etc...
84  */
85  double productIntegralWith(const CPoint2DPDFGaussian& p) const;
86 
87  /** Computes the "correspondence likelihood" of this PDF with another one:
88  * This is implemented as the integral from -inf to +inf of the product of
89  * both PDF.
90  * The resulting number is in the range [0,1].
91  * Note that the resulting value is in fact
92  * \f[ exp( -\frac{1}{2} D^2 ) \f]
93  * , with \f$ D^2 \f$ being the square Mahalanobis distance between the
94  * two pdfs.
95  * \sa productIntegralWith
96  * \exception std::exception On errors like covariance matrix with null
97  * determinant, etc...
98  */
100 
101  /** Draw a sample from the pdf */
102  void drawSingleSample(CPoint2D& outSample) const override;
103 
104  /** Bayesian fusion of two point distributions (product of two
105  * distributions->new distribution), then save the result in this object
106  * (WARNING: See implementing classes to see classes that can and cannot be
107  * mixtured!)
108  * \param p1 The first distribution to fuse
109  * \param p2 The second distribution to fuse
110  * \param minMahalanobisDistToDrop If set to different of 0, the result of
111  * very separate Gaussian modes (that will result in negligible components)
112  * in SOGs will be dropped to reduce the number of modes in the output.
113  */
114  void bayesianFusion(
115  const CPoint2DPDF& p1, const CPoint2DPDF& p2,
116  const double minMahalanobisDistToDrop = 0) override;
117 
118  /** Returns the Mahalanobis distance from this PDF to another PDF, that is,
119  * it's evaluation at (0,0,0) */
120  double mahalanobisDistanceTo(const CPoint2DPDFGaussian& other) const;
121  /** Returns the Mahalanobis distance from this PDF to some point */
122  double mahalanobisDistanceToPoint(const double x, const double y) const;
123 
124 }; // End of class def.
125 } // namespace mrpt::poses
double productIntegralNormalizedWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
double productIntegralWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
A gaussian distribution for 2D points.
void copyFrom(const CPoint2DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s particles to a text file, containing the 2D pose in the first line, then the covariance ma...
#define DEFINE_SCHEMA_SERIALIZABLE()
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void drawSingleSample(CPoint2D &outSample) const override
Draw a sample from the pdf.
void bayesianFusion(const CPoint2DPDFGaussian &p1, const CPoint2DPDFGaussian &p2)
Bayesian fusion of two points gauss.
Declares a class that represents a Probability Distribution function (PDF) of a 2D point (x...
Definition: CPoint2DPDF.h:33
double mahalanobisDistanceToPoint(const double x, const double y) const
Returns the Mahalanobis distance from this PDF to some point.
CMatrixFixed< double, 2, 2 > CMatrixDouble22
Definition: CMatrixFixed.h:364
A class used to store a 2D point.
Definition: CPoint2D.h:32
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void getMean(CPoint2D &p) const override
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the point covariance matrix (2x2 cov matrix) and the mean, both at once...
double mahalanobisDistanceTo(const CPoint2DPDFGaussian &other) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it&#39;s evaluation at (0...



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020