00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 CProbabilityDensityFunction_H 00029 #define CProbabilityDensityFunction_H 00030 00031 #include <mrpt/math/CMatrixD.h> 00032 #include <mrpt/math/CMatrixFixedNumeric.h> 00033 00034 namespace mrpt 00035 { 00036 namespace poses { class CPose3D; } 00037 00038 namespace utils 00039 { 00040 using namespace mrpt::math; 00041 00042 /** A generic template for probability density distributions (PDFs). 00043 * This template is used as base for many classes in mrpt::poses 00044 * Any derived class must implement \a getMean() and a getCovarianceAndMean(). 00045 * Other methods such as \a getMean() or \a getCovariance() are implemented here for convenience. 00046 * \sa mprt::poses::CPosePDF, mprt::poses::CPose3DPDF, mprt::poses::CPointPDF 00047 * \ingroup mrpt_base_grp 00048 */ 00049 template <class TDATA, size_t STATE_LEN> 00050 class CProbabilityDensityFunction 00051 { 00052 public: 00053 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). 00054 typedef TDATA type_value; //!< The type of the state the PDF represents 00055 00056 /** Returns the mean, or mathematical expectation of the probability density distribution (PDF). 00057 * \sa getCovarianceAndMean, getInformationMatrix 00058 */ 00059 virtual void getMean(TDATA &mean_point) const = 0; 00060 00061 /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once. 00062 * \sa getMean, getInformationMatrix 00063 */ 00064 virtual void getCovarianceAndMean(CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &cov,TDATA &mean_point) const = 0; 00065 00066 /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once. 00067 * \sa getMean, getInformationMatrix 00068 */ 00069 inline void getCovarianceDynAndMean(CMatrixDouble &cov,TDATA &mean_point) const 00070 { 00071 CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> C(UNINITIALIZED_MATRIX); 00072 this->getCovarianceAndMean(C,mean_point); 00073 cov = C; // Convert to dynamic size matrix 00074 } 00075 00076 /** Returns the mean, or mathematical expectation of the probability density distribution (PDF). 00077 * \sa getCovariance, getInformationMatrix 00078 */ 00079 inline TDATA getMeanVal() const 00080 { 00081 TDATA p; 00082 getMean(p); 00083 return p; 00084 } 00085 00086 /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) 00087 * \sa getMean, getCovarianceAndMean, getInformationMatrix 00088 */ 00089 inline void getCovariance(CMatrixDouble &cov) const 00090 { 00091 TDATA p; 00092 this->getCovarianceDynAndMean(cov,p); 00093 } 00094 00095 /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) 00096 * \sa getMean, getCovarianceAndMean, getInformationMatrix 00097 */ 00098 inline void getCovariance(CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &cov) const 00099 { 00100 TDATA p; 00101 this->getCovarianceAndMean(cov,p); 00102 } 00103 00104 /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) 00105 * \sa getMean, getInformationMatrix 00106 */ 00107 inline CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> getCovariance() const 00108 { 00109 CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> cov(UNINITIALIZED_MATRIX); 00110 TDATA p; 00111 this->getCovarianceAndMean(cov,p); 00112 return cov; 00113 } 00114 00115 00116 /** Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix) 00117 * Unless reimplemented in derived classes, this method first reads the covariance, then invert it. 00118 * \sa getMean, getCovarianceAndMean 00119 */ 00120 virtual void getInformationMatrix(CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &inf) const 00121 { 00122 CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> cov(UNINITIALIZED_MATRIX); 00123 TDATA p; 00124 this->getCovarianceAndMean(cov,p); 00125 cov.inv_fast(inf); // Destroy source cov matrix, since we don't need it anymore. 00126 } 00127 00128 /** Save PDF's particles to a text file. See derived classes for more information about the format of generated files. 00129 */ 00130 virtual void saveToTextFile(const std::string &file) const = 0; 00131 00132 /** Draws a single sample from the distribution 00133 */ 00134 virtual void drawSingleSample( TDATA &outPart ) const = 0; 00135 00136 /** 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. 00137 * This base method just call N times to drawSingleSample, but derived classes should implemented optimized method for each particular PDF. 00138 */ 00139 virtual void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const 00140 { 00141 outSamples.resize(N); 00142 TDATA pnt; 00143 for (size_t i=0;i<N;i++) 00144 { 00145 this->drawSingleSample(pnt); 00146 pnt.getAsVector(outSamples[i]); 00147 } 00148 } 00149 00150 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00151 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00152 */ 00153 virtual void changeCoordinatesReference( const mrpt::poses::CPose3D &newReferenceBase ) = 0; 00154 00155 /** Compute the entropy of the estimated covariance matrix. 00156 * \sa http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy 00157 */ 00158 inline double getCovarianceEntropy() const 00159 { 00160 static const double ln_2PI= 1.8378770664093454835606594728112; 00161 return 0.5*( STATE_LEN + STATE_LEN * ln_2PI + log( std::max(getCovariance().det(), std::numeric_limits<double>::epsilon() ) ) ); 00162 } 00163 00164 }; // End of class def. 00165 00166 } // End of namespace 00167 } // End of namespace 00168 00169 #endif
| Page generated by Doxygen 1.7.4 for MRPT 0.9.5 SVN:2717 at Sun Oct 16 16:08:03 PDT 2011 | | Hosted on: |