Main MRPT website > C++ reference
MRPT logo

CPoint2DPDFGaussian.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CPoint2DPDFGaussian_H
00029 #define CPoint2DPDFGaussian_H
00030 
00031 #include <mrpt/poses/CPoint2DPDF.h>
00032 
00033 namespace mrpt
00034 {
00035 namespace poses
00036 {
00037         using namespace mrpt::math;
00038 
00039         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPoint2DPDFGaussian, CPoint2DPDF )
00040 
00041         /** A gaussian distribution for 2D points. Also a method for bayesian fusion is provided.
00042      *
00043          * \sa CPoint2DPDF
00044          */
00045         class BASE_IMPEXP CPoint2DPDFGaussian : public CPoint2DPDF
00046         {
00047                 // This must be added to any CSerializable derived class:
00048                 DEFINE_SERIALIZABLE( CPoint2DPDFGaussian )
00049 
00050          public:
00051                 /** Default constructor
00052                   */
00053                 CPoint2DPDFGaussian();
00054 
00055                 /** Constructor
00056                   */
00057                 CPoint2DPDFGaussian( const CPoint2D &init_Mean );
00058 
00059                 /** Constructor
00060                   */
00061                 CPoint2DPDFGaussian( const CPoint2D &init_Mean, const CMatrixDouble22 &init_Cov );
00062 
00063                 /** The mean value
00064                  */
00065                 CPoint2D        mean;
00066 
00067                 /** The 2x2 covariance matrix
00068                  */
00069                 CMatrixDouble22         cov;
00070 
00071                  /** Returns an estimate of the point, (the mean, or mathematical expectation of the PDF)
00072                   */
00073                 void getMean(CPoint2D &p) const {
00074                         p = this->mean;
00075                 }
00076 
00077                 /** Returns an estimate of the point covariance matrix (2x2 cov matrix) and the mean, both at once.
00078                   * \sa getMean
00079                   */
00080                 void getCovarianceAndMean(CMatrixDouble22 &cov,CPoint2D &mean_point) const {
00081                         cov = this->cov;
00082                         mean_point = this->mean;
00083                 }
00084 
00085                 /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
00086                   */
00087                 void  copyFrom(const CPoint2DPDF &o);
00088 
00089                 /** Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance matrix in next 3 lines.
00090                  */
00091                 void  saveToTextFile(const std::string &file) const;
00092 
00093                 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
00094                   *   "to project" the current pdf. Result PDF substituted the currently stored one in the object. Both the mean value and the covariance matrix are updated correctly.
00095                   */
00096                 void  changeCoordinatesReference( const CPose3D &newReferenceBase );
00097 
00098                 /** Bayesian fusion of two points gauss. distributions, then save the result in this object.
00099                   *  The process is as follows:<br>
00100                   *             - (x1,S1): Mean and variance of the p1 distribution.
00101                   *             - (x2,S2): Mean and variance of the p2 distribution.
00102                   *             - (x,S): Mean and variance of the resulting distribution.
00103                   *
00104                   *    S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>;
00105                   *    x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 );
00106                   */
00107                 void  bayesianFusion( const CPoint2DPDFGaussian &p1, const CPoint2DPDFGaussian &p2 );
00108 
00109                 /** Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the integral from -inf to +inf of the product of both PDF.
00110                   * The resulting number is >=0.
00111                   * \sa productIntegralNormalizedWith
00112                   * \exception std::exception On errors like covariance matrix with null determinant, etc...
00113                   */
00114                 double  productIntegralWith( const CPoint2DPDFGaussian &p) const;
00115 
00116                 /** Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the integral from -inf to +inf of the product of both PDF.
00117                   * The resulting number is in the range [0,1]. 
00118                   *  Note that the resulting value is in fact 
00119                   *  \f[ exp( -\frac{1}{2} D^2 ) \f] 
00120                   *  , with \f$ D^2 \f$ being the square Mahalanobis distance between the two pdfs.
00121                   * \sa productIntegralWith
00122                   * \exception std::exception On errors like covariance matrix with null determinant, etc...
00123                   */
00124                 double  productIntegralNormalizedWith( const CPoint2DPDFGaussian &p) const;
00125 
00126                 /** Draw a sample from the pdf.
00127                   */
00128                 void drawSingleSample(CPoint2D  &outSample) const;
00129 
00130                 /** Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
00131                   * \param p1 The first distribution to fuse
00132                   * \param p2 The second distribution to fuse
00133                   * \param minMahalanobisDistToDrop If set to different of 0, the result of very separate Gaussian modes (that will result in negligible components) in SOGs will be dropped to reduce the number of modes in the output.
00134                   */
00135                 void  bayesianFusion( const CPoint2DPDF &p1, const CPoint2DPDF &p2, const double &minMahalanobisDistToDrop = 0);
00136 
00137 
00138                 /** Returns the Mahalanobis distance from this PDF to another PDF, that is, it's evaluation at (0,0,0)
00139                   */
00140                 double mahalanobisDistanceTo( const CPoint2DPDFGaussian & other ) const;
00141 
00142 
00143         }; // End of class def.
00144 
00145 
00146         } // End of namespace
00147 } // End of namespace
00148 
00149 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011