Main MRPT website > C++ reference
MRPT logo
CPosePDFGaussianInf.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPosePDFGaussianInf_H
29 #define CPosePDFGaussianInf_H
30 
31 #include <mrpt/poses/CPosePDF.h>
32 #include <mrpt/math/CMatrixFixedNumeric.h>
33 
34 namespace mrpt
35 {
36 namespace poses
37 {
38  using namespace mrpt::math;
39 
40  class CPose3DPDF;
41 
42  // This must be added to any CSerializable derived class:
43  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFGaussianInf, CPosePDF )
44 
45  /** A Probability Density function (PDF) of a 2D pose \f$ p(\mathbf{x}) = [x ~ y ~ \phi ]^t \f$ as a Gaussian with a mean and the inverse of the covariance.
46  *
47  * This class implements a PDF as a mono-modal Gaussian distribution in its <b>information form</b>, that is,
48  * keeping the inverse of the covariance matrix instead of the covariance matrix itself.
49  *
50  * This class is the dual of CPosePDFGaussian.
51  *
52  * \sa CPose2D, CPosePDF, CPosePDFParticles
53  * \ingroup poses_pdf_grp
54  */
55  class BASE_IMPEXP CPosePDFGaussianInf : public CPosePDF
56  {
57  // This must be added to any CSerializable derived class:
58  DEFINE_SERIALIZABLE( CPosePDFGaussianInf )
59 
60  protected:
61  /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)
62  */
63  void assureSymmetry();
64 
65  public:
66  /** @name Data fields
67  @{ */
68 
69  CPose2D mean; //!< The mean value
70  CMatrixDouble33 cov_inv; //!< The inverse of the 3x3 covariance matrix (the "information" matrix)
71 
72  /** @} */
73 
74  inline const CPose2D & getPoseMean() const { return mean; }
75  inline CPose2D & getPoseMean() { return mean; }
76 
77  /** Default constructor (mean=all zeros, inverse covariance=all zeros -> so be careful!) */
78  CPosePDFGaussianInf();
79 
80  /** Constructor with a mean value (inverse covariance=all zeros -> so be careful!) */
81  explicit CPosePDFGaussianInf( const CPose2D &init_Mean );
82 
83  /** Constructor */
84  CPosePDFGaussianInf( const CPose2D &init_Mean, const CMatrixDouble33 &init_CovInv );
85 
86  /** Copy constructor, including transformations between other PDFs */
87  explicit CPosePDFGaussianInf( const CPosePDF &o ) { copyFrom( o ); }
88 
89  /** Copy constructor, including transformations between other PDFs */
90  explicit CPosePDFGaussianInf( const CPose3DPDF &o ) { copyFrom( o ); }
91 
92 
93  /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
94  * \sa getCovariance
95  */
96  void getMean(CPose2D &mean_pose) const {
97  mean_pose = mean;
98  }
99 
100  /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
101  * \sa getMean
102  */
103  void getCovarianceAndMean(CMatrixDouble33 &cov,CPose2D &mean_point) const {
104  mean_point = mean;
105  this->cov_inv.inv(cov);
106  }
107 
108  /** Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix) \sa getMean, getCovarianceAndMean */
109  virtual void getInformationMatrix(CMatrixDouble33 &inf) const { inf=cov_inv; }
110 
111  /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
112  */
113  void copyFrom(const CPosePDF &o);
114 
115  /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
116  */
117  void copyFrom(const CPose3DPDF &o);
118 
119  /** Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance matrix in next 3 lines.
120  */
121  void saveToTextFile(const std::string &file) const;
122 
123  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
124  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
125  */
126  void changeCoordinatesReference( const CPose3D &newReferenceBase );
127 
128  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
129  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
130  */
131  void changeCoordinatesReference( const CPose2D &newReferenceBase );
132 
133  /** 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$.
134  */
135  void rotateCov(const double ang);
136 
137  /** 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!).
138  */
139  void inverseComposition( const CPosePDFGaussianInf &x, const CPosePDFGaussianInf &ref );
140 
141  /** 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).
142  */
143  void inverseComposition(
144  const CPosePDFGaussianInf &x1,
145  const CPosePDFGaussianInf &x0,
146  const CMatrixDouble33 &COV_01
147  );
148 
149  /** Draws a single sample from the distribution
150  */
151  void drawSingleSample( CPose2D &outPart ) const;
152 
153  /** 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.
154  */
155  void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const;
156 
157  /** Bayesian fusion of two points gauss. distributions, then save the result in this object.
158  * The process is as follows:<br>
159  * - (x1,S1): Mean and variance of the p1 distribution.
160  * - (x2,S2): Mean and variance of the p2 distribution.
161  * - (x,S): Mean and variance of the resulting distribution.
162  *
163  * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>;
164  * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 );
165  */
166  void bayesianFusion(const CPosePDF &p1,const CPosePDF &p2, const double &minMahalanobisDistToDrop = 0 );
167 
168  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
169  */
170  void inverse(CPosePDF &o) const;
171 
172  /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). */
173  void operator += ( const CPose2D &Ap);
174 
175  /** Evaluates the PDF at a given point.
176  */
177  double evaluatePDF( const CPose2D &x ) const;
178 
179  /** Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
180  */
181  double evaluateNormalizedPDF( const CPose2D &x ) const;
182 
183  /** Computes the Mahalanobis distance between the centers of two Gaussians.
184  */
185  double mahalanobisDistanceTo( const CPosePDFGaussianInf& theOther );
186 
187  /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated) (see formulas in jacobiansPoseComposition ).
188  */
189  void operator += ( const CPosePDFGaussianInf &Ap);
190 
191  /** Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated)
192  */
193  inline void operator -=( const CPosePDFGaussianInf &ref ) {
194  this->inverseComposition(*this,ref);
195  }
196 
197  }; // End of class def.
198 
199 
200  /** Pose compose operator: RES = A (+) B , computing both the mean and the covariance */
201  inline CPosePDFGaussianInf operator +( const CPosePDFGaussianInf &a, const CPosePDFGaussianInf &b ) {
202  CPosePDFGaussianInf res(a);
203  res+=b;
204  return res;
205  }
206 
207  /** Pose inverse compose operator: RES = A (-) B , computing both the mean and the covariance */
208  inline CPosePDFGaussianInf operator -( const CPosePDFGaussianInf &a, const CPosePDFGaussianInf &b ) {
209  CPosePDFGaussianInf res;
210  res.inverseComposition(a,b);
211  return res;
212  }
213 
214  /** Dumps the mean and covariance matrix to a text stream.
215  */
216  std::ostream BASE_IMPEXP & operator << (std::ostream & out, const CPosePDFGaussianInf& obj);
217 
218  /** Returns the Gaussian distribution of \f$ \mathbf{C} \f$, for \f$ \mathbf{C} = \mathbf{A} \oplus \mathbf{B} \f$.
219  */
220  poses::CPosePDFGaussianInf BASE_IMPEXP operator + ( const mrpt::poses::CPose2D &A, const mrpt::poses::CPosePDFGaussianInf &B );
221 
222  bool BASE_IMPEXP operator==(const CPosePDFGaussianInf &p1,const CPosePDFGaussianInf &p2);
223 
224  } // End of namespace
225 } // End of namespace
226 
227 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013