00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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 MRPTDLLIMPEXP 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( CPointPDFGaussian &p1, 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( 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( CPointPDF &p1, 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.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |