Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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:
23  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFGaussian, CPosePDF )
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  */
40  void assureSymmetry();
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 {
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);
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;
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 );
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!). */
111  void inverseComposition( const CPosePDFGaussian &x, const CPosePDFGaussian &ref );
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). */
114  void inverseComposition(
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;
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;
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 );
138 
139  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
140  */
141  void inverse(CPosePDF &o) const;
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 */
167  void composePoint(const mrpt::math::TPoint2D &l, CPoint2DPDFGaussian &g ) const;
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$. */
185 
186  bool BASE_IMPEXP operator==(const CPosePDFGaussian &p1,const CPosePDFGaussian &p2);
187 
188  } // End of namespace
189 } // End of namespace
190 
191 #endif
void getMean(CPose2D &mean_pose) const
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
CPosePDFGaussian(const CPosePDF &o)
Copy constructor, including transformations between other PDFs.
CPose2D BASE_IMPEXP operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
A gaussian distribution for 2D points.
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
A numeric matrix of compile-time fixed size.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
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
CPosePDFGaussian(const CPose3DPDF &o)
Copy constructor, including transformations between other PDFs.
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
mrpt::math::TPoint2D BASE_IMPEXP operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once...
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...
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:130
A class used to store a 2D pose.
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
Lightweight 2D point.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].
Definition: CPoint.h:106



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Thu Nov 10 13:46:27 UTC 2016