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.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |