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 
15 namespace mrpt
16 {
17 namespace poses
18 {
19  class CPose3DPDF;
20  class CPoint2DPDFGaussian;
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) */
88  void copyFrom(const CPosePDF &o) MRPT_OVERRIDE;
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  */
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.
const CPose2D & getPoseMean() const
void changeCoordinatesReference(const CPose2D &newReferenceBase)
this = p (+) this.
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
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
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > 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:135
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D point.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Sat Jan 30 21:34:41 UTC 2021