Main MRPT website > C++ reference for MRPT 1.4.0
CPosePDFGaussian.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-2016, 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 CPosePDFGaussian_H
10#define CPosePDFGaussian_H
11
12#include <mrpt/poses/CPosePDF.h>
14
15namespace mrpt
16{
17namespace poses
18{
19 class CPose3DPDF;
21
22 // This must be added to any CSerializable derived class:
24
25 /** Declares a class that represents a Probability Density function (PDF) of a 2D pose \f$ p(\mathbf{x}) = [x ~ y ~ \phi ]^t \f$.
26 *
27 * This class implements that PDF using a mono-modal Gaussian distribution. See mrpt::poses::CPosePDF for more details.
28 *
29 * \sa CPose2D, CPosePDF, CPosePDFParticles
30 * \ingroup poses_pdf_grp
31 */
33 {
34 // This must be added to any CSerializable derived class:
36
37 protected:
38 /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)
39 */
41
42 public:
43 /** @name Data fields
44 @{ */
45
46 CPose2D mean; //!< The mean value
47 mrpt::math::CMatrixDouble33 cov; //!< The 3x3 covariance matrix
48
49 /** @} */
50
51 inline const CPose2D & getPoseMean() const { return mean; }
52 inline CPose2D & getPoseMean() { return mean; }
53
54 /** Default constructor
55 */
57
58 /** Constructor
59 */
60 explicit CPosePDFGaussian( const CPose2D &init_Mean );
61
62 /** Constructor
63 */
64 CPosePDFGaussian( const CPose2D &init_Mean, const mrpt::math::CMatrixDouble33 &init_Cov );
65
66 /** Copy constructor, including transformations between other PDFs */
67 explicit CPosePDFGaussian( const CPosePDF &o ) { copyFrom( o ); }
68
69 /** Copy constructor, including transformations between other PDFs */
70 explicit CPosePDFGaussian( const CPose3DPDF &o ) { copyFrom( o ); }
71
72 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
73 * \sa getCovariance
74 */
75 void getMean(CPose2D &mean_pose) const MRPT_OVERRIDE{
76 mean_pose = mean;
77 }
78
79 /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
80 * \sa getMean
81 */
83 mean_point = mean;
84 cov = this->cov;
85 }
86
87 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) */
89
90 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) */
91 void copyFrom(const CPose3DPDF &o);
92
93 /** Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance matrix in next 3 lines. */
94 void saveToTextFile(const std::string &file) const MRPT_OVERRIDE;
95
96 /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
97 * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
98 */
99 void changeCoordinatesReference( const CPose3D &newReferenceBase ) MRPT_OVERRIDE;
100
101 /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
102 * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
103 */
104 void changeCoordinatesReference( const CPose2D &newReferenceBase );
105
106 /** Rotate the covariance matrix by replacing it by \f$ \mathbf{R}~\mathbf{COV}~\mathbf{R}^t \f$, where \f$ \mathbf{R} = \left[ \begin{array}{ccc} \cos\alpha & -\sin\alpha & 0 \\ \sin\alpha & \cos\alpha & 0 \\ 0 & 0 & 1 \end{array}\right] \f$.
107 */
108 void rotateCov(const double ang);
109
110 /** Set \f$ this = x1 \ominus x0 \f$ , computing the mean using the "-" operator and the covariances through the corresponding Jacobians (For 'x0' and 'x1' being independent variables!). */
112
113 /** Set \f$ this = x1 \ominus x0 \f$ , computing the mean using the "-" operator and the covariances through the corresponding Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x1). */
115 const CPosePDFGaussian &x1,
116 const CPosePDFGaussian &x0,
117 const mrpt::math::CMatrixDouble33 &COV_01
118 );
119
120 /** Draws a single sample from the distribution
121 */
122 void drawSingleSample( CPose2D &outPart ) const MRPT_OVERRIDE;
123
124 /** Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum.
125 */
126 void drawManySamples( size_t N, std::vector<mrpt::math::CVectorDouble> & outSamples ) const MRPT_OVERRIDE;
127
128 /** Bayesian fusion of two points gauss. distributions, then save the result in this object.
129 * The process is as follows:<br>
130 * - (x1,S1): Mean and variance of the p1 distribution.
131 * - (x2,S2): Mean and variance of the p2 distribution.
132 * - (x,S): Mean and variance of the resulting distribution.
133 *
134 * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>;
135 * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 );
136 */
137 void bayesianFusion(const CPosePDF &p1,const CPosePDF &p2, const double &minMahalanobisDistToDrop = 0 ) MRPT_OVERRIDE;
138
139 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
140 */
141 void inverse(CPosePDF &o) const MRPT_OVERRIDE;
142
143 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). */
144 void operator += ( const CPose2D &Ap);
145
146 /** Evaluates the PDF at a given point. */
147 double evaluatePDF( const CPose2D &x ) const;
148
149 /** Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1]. */
150 double evaluateNormalizedPDF( const CPose2D &x ) const;
151
152 /** Computes the Mahalanobis distance between the centers of two Gaussians. */
153 double mahalanobisDistanceTo( const CPosePDFGaussian& theOther );
154
155 /** Substitutes the diagonal elements if (square) they are below some given minimum values (Use this before bayesianFusion, for example, to avoid inversion of singular matrixes, etc...) */
156 void assureMinCovariance( const double & minStdXY, const double &minStdPhi );
157
158 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated) (see formulas in jacobiansPoseComposition ). */
159 void operator += ( const CPosePDFGaussian &Ap);
160
161 /** Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated) */
162 inline void operator -=( const CPosePDFGaussian &ref ) {
163 this->inverseComposition(*this,ref);
164 }
165
166 /** Returns the PDF of the 2D point \f$ g = q \oplus l\f$ with "q"=this pose and "l" a point without uncertainty */
168
169
170 }; // End of class def.
172
173
174 /** Pose compose operator: RES = A (+) B , computing both the mean and the covariance */
176
177 /** Pose inverse compose operator: RES = A (-) B , computing both the mean and the covariance */
179
180 /** Dumps the mean and covariance matrix to a text stream. */
181 std::ostream BASE_IMPEXP & operator << (std::ostream & out, const CPosePDFGaussian& obj);
182
183 /** Returns the Gaussian distribution of \f$ \mathbf{C} \f$, for \f$ \mathbf{C} = \mathbf{A} \oplus \mathbf{B} \f$. */
184 poses::CPosePDFGaussian BASE_IMPEXP operator + ( const mrpt::poses::CPose2D &A, const mrpt::poses::CPosePDFGaussian &B );
185
186 bool BASE_IMPEXP operator==(const CPosePDFGaussian &p1,const CPosePDFGaussian &p2);
187
188 } // End of namespace
189} // End of namespace
190
191#endif
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
A numeric matrix of compile-time fixed size.
A gaussian distribution for 2D points.
A class used to store a 2D pose.
Definition CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition CPose3D.h:73
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
Definition CPose3DPDF.h:41
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
CPosePDFGaussian()
Default constructor.
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const MRPT_OVERRIDE
Draws a number of samples from the distribution, and saves as a list of 1x3 vectors,...
CPose2D mean
The mean value.
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
CPosePDFGaussian(const CPosePDF &o)
Copy constructor, including transformations between other PDFs.
void copyFrom(const CPose3DPDF &o)
Copy operator, translating if necesary (for example, between particles and gaussian representations)
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations)
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
CPosePDFGaussian(const CPose2D &init_Mean, const mrpt::math::CMatrixDouble33 &init_Cov)
Constructor.
CPosePDFGaussian(const CPose2D &init_Mean)
Constructor.
void inverseComposition(const CPosePDFGaussian &x, const CPosePDFGaussian &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jacobi...
CPosePDFGaussian(const CPose3DPDF &o)
Copy constructor, including transformations between other PDFs.
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance ma...
void composePoint(const mrpt::math::TPoint2D &l, CPoint2DPDFGaussian &g) const
Returns the PDF of the 2D point with "q"=this pose and "l" a point without uncertainty.
void getMean(CPose2D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
void drawSingleSample(CPose2D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE
Bayesian fusion of two points gauss.
void changeCoordinatesReference(const CPose2D &newReferenceBase)
this = p (+) this.
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
const CPose2D & getPoseMean() const
void inverseComposition(const CPosePDFGaussian &x1, const CPosePDFGaussian &x0, const mrpt::math::CMatrixDouble33 &COV_01)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jacobi...
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
Definition CPosePDF.h:40
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition mrpt_macros.h:28
class BASE_IMPEXP CPoint2DPDFGaussian
class BASE_IMPEXP CPose3DPDF
Definition CPose3DPDF.h:23
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
Lightweight 2D point.



Page generated by Doxygen 1.9.8 for MRPT 1.4.0 SVN: at Wed Dec 6 15:06:50 UTC 2023