Main MRPT website > C++ reference
MRPT logo

CPointPDFGaussian.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 CPointPDFGaussian_H
00029 #define CPointPDFGaussian_H
00030 
00031 #include <mrpt/poses/CPointPDF.h>
00032 #include <mrpt/math/CMatrix.h>
00033 
00034 namespace mrpt
00035 {
00036 namespace poses
00037 {
00038         using namespace mrpt::math;
00039 
00040         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointPDFGaussian, CPointPDF )
00041 
00042         /** A gaussian distribution for 3D points. Also a method for bayesian fusion is provided.
00043      *
00044          * \sa CPointPDF
00045          */
00046         class BASE_IMPEXP CPointPDFGaussian : public CPointPDF
00047         {
00048                 // This must be added to any CSerializable derived class:
00049                 DEFINE_SERIALIZABLE( CPointPDFGaussian )
00050 
00051          public:
00052                 /** Default constructor
00053                   */
00054                 CPointPDFGaussian();
00055 
00056                 /** Constructor
00057                   */
00058                 CPointPDFGaussian( const CPoint3D &init_Mean );
00059 
00060                 /** Constructor
00061                   */
00062                 CPointPDFGaussian( const CPoint3D &init_Mean, const CMatrixDouble33 &init_Cov );
00063 
00064                 /** The mean value
00065                  */
00066                 CPoint3D        mean;
00067 
00068                 /** The 3x3 covariance matrix
00069                  */
00070                 CMatrixDouble33         cov;
00071 
00072                  /** Returns an estimate of the point, (the mean, or mathematical expectation of the PDF)
00073                   */
00074                 void getMean(CPoint3D &p) const;
00075 
00076                 /** Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once.
00077                   * \sa getMean
00078                   */
00079                 void getCovarianceAndMean(CMatrixDouble33 &cov,CPoint3D &mean_point) const;
00080 
00081                 /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
00082                   */
00083                 void  copyFrom(const CPointPDF &o);
00084 
00085                 /** Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance matrix in next 3 lines.
00086                  */
00087                 void  saveToTextFile(const std::string &file) const;
00088 
00089                 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
00090                   *   "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.
00091                   */
00092                 void  changeCoordinatesReference( const CPose3D &newReferenceBase );
00093 
00094                 /** Bayesian fusion of two points gauss. distributions, then save the result in this object.
00095                   *  The process is as follows:<br>
00096                   *             - (x1,S1): Mean and variance of the p1 distribution.
00097                   *             - (x2,S2): Mean and variance of the p2 distribution.
00098                   *             - (x,S): Mean and variance of the resulting distribution.
00099                   *
00100                   *    S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>;
00101                   *    x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 );
00102                   */
00103                 void  bayesianFusion( const CPointPDFGaussian &p1, const CPointPDFGaussian &p2 );
00104 
00105                 /** 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.
00106                   * The resulting number is >=0.
00107                   * \sa productIntegralNormalizedWith
00108                   * \exception std::exception On errors like covariance matrix with null determinant, etc...
00109                   */
00110                 double  productIntegralWith( const CPointPDFGaussian &p) const;
00111 
00112                 /** 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.
00113                   * The resulting number is >=0.
00114                   * NOTE: This version ignores the "z" coordinates!!
00115                   * \sa productIntegralNormalizedWith
00116                   * \exception std::exception On errors like covariance matrix with null determinant, etc...
00117                   */
00118                 double  productIntegralWith2D( const CPointPDFGaussian &p) const;
00119 
00120                 /** 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.
00121                   * The resulting number is in the range [0,1]
00122                   *  Note that the resulting value is in fact
00123                   *  \f[ exp( -\frac{1}{2} D^2 ) \f]
00124                   *  , with \f$ D^2 \f$ being the square Mahalanobis distance between the two pdfs.
00125                   * \sa productIntegralWith
00126                   * \exception std::exception On errors like covariance matrix with null determinant, etc...
00127                   */
00128                 double  productIntegralNormalizedWith( const CPointPDFGaussian &p) const;
00129 
00130                 /** 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.
00131                   * The resulting number is in the range [0,1]. This versions ignores the "z" coordinate.
00132                   *
00133                   *  Note that the resulting value is in fact
00134                   *  \f[ exp( -\frac{1}{2} D^2 ) \f]
00135                   *  , with \f$ D^2 \f$ being the square Mahalanobis distance between the two pdfs.
00136                   * \sa productIntegralWith
00137                   * \exception std::exception On errors like covariance matrix with null determinant, etc...
00138                   */
00139                 double  productIntegralNormalizedWith2D( const CPointPDFGaussian &p) const;
00140 
00141                 /** Draw a sample from the pdf.
00142                   */
00143                 void drawSingleSample(CPoint3D  &outSample) const;
00144 
00145                 /** 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!)
00146                   * \param p1 The first distribution to fuse
00147                   * \param p2 The second distribution to fuse
00148                   * \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.
00149                   */
00150                 void  bayesianFusion( const CPointPDF &p1,const  CPointPDF &p2, const double &minMahalanobisDistToDrop = 0);
00151 
00152 
00153                 /** Returns the Mahalanobis distance from this PDF to another PDF, that is, it's evaluation at (0,0,0)
00154                   */
00155                 double mahalanobisDistanceTo( const CPointPDFGaussian & other, bool only_2D = false ) const;
00156 
00157 
00158         }; // End of class def.
00159 
00160 
00161         } // End of namespace
00162 } // End of namespace
00163 
00164 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011