Main MRPT website > C++ reference
MRPT logo

CProbabilityDensityFunction.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 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                         inline 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                         inline 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                         inline 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                         inline 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                         inline 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                         inline 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.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011