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 CProbabilityDensityFunction_H 00029 #define CProbabilityDensityFunction_H 00030 00031 #include <mrpt/math/CMatrixD.h> 00032 00033 namespace mrpt 00034 { 00035 namespace poses { class CPose3D; } 00036 00037 namespace utils 00038 { 00039 using namespace mrpt::math; 00040 00041 /** A generic template for probability density distributions (PDFs). 00042 * This template is used as base for many classes in mrpt::poses 00043 * Any derived class must implement "getMean" and "getCovarianceAndMean". 00044 * Other methods such as "getCovariance" are implemented here for convenience. 00045 * \sa mprt::poses::CPosePDF, mprt::poses::CPose3DPDF, mprt::poses::CPointPDF 00046 */ 00047 template <class TDATA, size_t STATE_LEN> 00048 class CProbabilityDensityFunction 00049 { 00050 public: 00051 static const size_t state_length = STATE_LEN; //!< The length of the variable, for example, 3 for a 3D point, 6 for a 3D pose (x y z yaw pitch roll). 00052 typedef TDATA type_value; //!< The type of the state the PDF represents 00053 00054 /** Returns the mean, or mathematical expectation of the probability density distribution (PDF). 00055 * \sa getCovarianceAndMean 00056 */ 00057 virtual void getMean(TDATA &mean_point) const = 0; 00058 00059 /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once. 00060 * \sa getMean 00061 */ 00062 virtual void getCovarianceAndMean(CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &cov,TDATA &mean_point) const = 0; 00063 00064 /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once. 00065 * \sa getMean 00066 */ 00067 void getCovarianceDynAndMean(CMatrixDouble &cov,TDATA &mean_point) const 00068 { 00069 CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> C; 00070 this->getCovarianceAndMean(C,mean_point); 00071 cov = C; // Convert to dynamic size matrix 00072 } 00073 00074 /** Returns the mean, or mathematical expectation of the probability density distribution (PDF). 00075 * \sa getCovariance 00076 */ 00077 TDATA getMeanVal() const 00078 { 00079 TDATA p; 00080 getMean(p); 00081 return p; 00082 } 00083 00084 /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) 00085 * \sa getMean, getCovarianceAndMean 00086 */ 00087 void getCovariance(CMatrixDouble &cov) const 00088 { 00089 TDATA p; 00090 this->getCovarianceDynAndMean(cov,p); 00091 } 00092 00093 /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) 00094 * \sa getMean, getCovarianceAndMean 00095 */ 00096 void getCovariance(CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &cov) const 00097 { 00098 TDATA p; 00099 this->getCovarianceAndMean(cov,p); 00100 } 00101 00102 /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) 00103 * \sa getMean 00104 */ 00105 CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> getCovariance() const 00106 { 00107 CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> cov; 00108 TDATA p; 00109 this->getCovarianceAndMean(cov,p); 00110 return cov; 00111 } 00112 00113 /** Save PDF's particles to a text file. See derived classes for more information about the format of generated files. 00114 */ 00115 virtual void saveToTextFile(const std::string &file) const = 0; 00116 00117 /** Draws a single sample from the distribution 00118 */ 00119 virtual void drawSingleSample( TDATA &outPart ) const = 0; 00120 00121 /** Draws a number of samples from the distribution, and saves as a list of 1xSTATE_LEN vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum. 00122 * This base method just call N times to drawSingleSample, but derived classes should implemented optimized method for each particular PDF. 00123 */ 00124 virtual void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const 00125 { 00126 outSamples.resize(N); 00127 TDATA pnt; 00128 for (size_t i=0;i<N;i++) 00129 { 00130 this->drawSingleSample(pnt); 00131 pnt.getAsVector(outSamples[i]); 00132 } 00133 } 00134 00135 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00136 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00137 */ 00138 virtual void changeCoordinatesReference( const mrpt::poses::CPose3D &newReferenceBase ) = 0; 00139 00140 /** Compute the entropy of the estimated covariance matrix. 00141 * \sa http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy 00142 */ 00143 double getCovarianceEntropy() const 00144 { 00145 static const double ln_2PI= 1.8378770664093454835606594728112; 00146 return 0.5*( STATE_LEN + STATE_LEN * ln_2PI + log( std::max(getCovariance().det(), std::numeric_limits<double>::epsilon() ) ) ); 00147 } 00148 00149 /** Returns an estimate of the covariance matrix 00150 * \note Deprecated: Use getCovariance or getCovarianceAndMean instead. 00151 */ 00152 MRPT_DECLARE_DEPRECATED_FUNCTION("**deprecated** Use getCovariance instead", 00153 CMatrixD getEstimatedCovariance() const ) 00154 { 00155 CMatrixDouble C; 00156 getCovariance(C); 00157 return CMatrixD(C); 00158 } 00159 00160 }; // End of class def. 00161 00162 } // End of namespace 00163 } // End of namespace 00164 00165 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:02:22 EDT 2009 |