Main MRPT website > C++ reference for MRPT 1.9.9
CPointPDFGaussian.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, 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 #ifndef CPointPDFGaussian_H
10 #define CPointPDFGaussian_H
11 
12 #include <mrpt/poses/CPointPDF.h>
13 #include <mrpt/math/CMatrix.h>
14 
15 namespace mrpt
16 {
17 namespace poses
18 {
19 /** A gaussian distribution for 3D points. Also a method for bayesian fusion is
20  * provided.
21  *
22  * \sa CPointPDF
23  * \ingroup poses_pdf_grp
24  */
26 {
28 
29  public:
30  /** Default constructor
31  */
33 
34  /** Constructor
35  */
36  CPointPDFGaussian(const CPoint3D& init_Mean);
37 
38  /** Constructor
39  */
41  const CPoint3D& init_Mean, const mrpt::math::CMatrixDouble33& init_Cov);
42 
43  /** The mean value */
45  /** The 3x3 covariance matrix */
47 
48  /** Returns an estimate of the point, (the mean, or mathematical expectation
49  * of the PDF) */
50  void getMean(CPoint3D& p) const override;
51 
52  /** Returns an estimate of the point covariance matrix (3x3 cov matrix) and
53  * the mean, both at once. \sa getMean */
55  mrpt::math::CMatrixDouble33& cov, CPoint3D& mean_point) const override;
56 
57  /** Copy operator, translating if necesary (for example, between particles
58  * and gaussian representations) */
59  void copyFrom(const CPointPDF& o) override;
60 
61  /** Save PDF's particles to a text file, containing the 2D pose in the first
62  * line, then the covariance matrix in next 3 lines. */
63  void saveToTextFile(const std::string& file) const override;
64 
65  /** this = p (+) this. This can be used to convert a PDF from local
66  * coordinates to global, providing the point (newReferenceBase) from which
67  * "to project" the current pdf. Result PDF substituted the currently
68  * stored one in the object. Both the mean value and the covariance matrix
69  * are updated correctly. */
70  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
71 
72  /** Bayesian fusion of two points gauss. distributions, then save the result
73  *in this object.
74  * The process is as follows:<br>
75  * - (x1,S1): Mean and variance of the p1 distribution.
76  * - (x2,S2): Mean and variance of the p2 distribution.
77  * - (x,S): Mean and variance of the resulting distribution.
78  *
79  * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>;
80  * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 );
81  */
82  void bayesianFusion(
83  const CPointPDFGaussian& p1, const CPointPDFGaussian& p2);
84 
85  /** Computes the "correspondence likelihood" of this PDF with another one:
86  * This is implemented as the integral from -inf to +inf of the product of
87  * both PDF.
88  * The resulting number is >=0.
89  * \sa productIntegralNormalizedWith
90  * \exception std::exception On errors like covariance matrix with null
91  * determinant, etc...
92  */
93  double productIntegralWith(const CPointPDFGaussian& p) const;
94 
95  /** Computes the "correspondence likelihood" of this PDF with another one:
96  * This is implemented as the integral from -inf to +inf of the product of
97  * both PDF.
98  * The resulting number is >=0.
99  * NOTE: This version ignores the "z" coordinates!!
100  * \sa productIntegralNormalizedWith
101  * \exception std::exception On errors like covariance matrix with null
102  * determinant, etc...
103  */
104  double productIntegralWith2D(const CPointPDFGaussian& p) const;
105 
106  /** Computes the "correspondence likelihood" of this PDF with another one:
107  * This is implemented as the integral from -inf to +inf of the product of
108  * both PDF.
109  * The resulting number is in the range [0,1]
110  * Note that the resulting value is in fact
111  * \f[ exp( -\frac{1}{2} D^2 ) \f]
112  * , with \f$ D^2 \f$ being the square Mahalanobis distance between the
113  * two pdfs.
114  * \sa productIntegralWith
115  * \exception std::exception On errors like covariance matrix with null
116  * determinant, etc...
117  */
119 
120  /** Computes the "correspondence likelihood" of this PDF with another one:
121  * This is implemented as the integral from -inf to +inf of the product of
122  * both PDF.
123  * The resulting number is in the range [0,1]. This versions ignores the
124  * "z" coordinate.
125  *
126  * Note that the resulting value is in fact
127  * \f[ exp( -\frac{1}{2} D^2 ) \f]
128  * , with \f$ D^2 \f$ being the square Mahalanobis distance between the
129  * two pdfs.
130  * \sa productIntegralWith
131  * \exception std::exception On errors like covariance matrix with null
132  * determinant, etc...
133  */
135 
136  /** Draw a sample from the pdf */
137  void drawSingleSample(CPoint3D& outSample) const override;
138 
139  /** Bayesian fusion of two point distributions (product of two
140  * distributions->new distribution), then save the result in this object
141  * (WARNING: See implementing classes to see classes that can and cannot be
142  * mixtured!)
143  * \param p1 The first distribution to fuse
144  * \param p2 The second distribution to fuse
145  * \param minMahalanobisDistToDrop If set to different of 0, the result of
146  * very separate Gaussian modes (that will result in negligible components)
147  * in SOGs will be dropped to reduce the number of modes in the output.
148  */
149  void bayesianFusion(
150  const CPointPDF& p1, const CPointPDF& p2,
151  const double& minMahalanobisDistToDrop = 0) override;
152 
153  /** Returns the Mahalanobis distance from this PDF to another PDF, that is,
154  * it's evaluation at (0,0,0) */
155  double mahalanobisDistanceTo(
156  const CPointPDFGaussian& other, bool only_2D = false) const;
157 
158 }; // End of class def.
159 } // End of namespace
160 } // End of namespace
161 
162 #endif
void 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...
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void getMean(CPoint3D &p) const override
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
CPoint3D mean
The mean value.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
double productIntegralNormalizedWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 3D point.
Definition: CPoint3D.h:32
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
double mahalanobisDistanceTo(const CPointPDFGaussian &other, bool only_2D=false) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it&#39;s evaluation at (0...
double productIntegralWith(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:39
GLfloat GLfloat p
Definition: glext.h:6305
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const override
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once...
CPointPDFGaussian()
Default constructor.
double productIntegralWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
A gaussian distribution for 3D points.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019