Main MRPT website > C++ reference
MRPT logo
CProbabilityDensityFunction.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CProbabilityDensityFunction_H
29 #define CProbabilityDensityFunction_H
30 
31 #include <mrpt/math/CMatrixD.h>
32 #include <mrpt/math/CMatrixFixedNumeric.h>
33 
34 namespace mrpt
35 {
36  namespace poses { class CPose3D; }
37 
38  namespace utils
39  {
40  using namespace mrpt::math;
41 
42  /** A generic template for probability density distributions (PDFs).
43  * This template is used as base for many classes in mrpt::poses
44  * Any derived class must implement \a getMean() and a getCovarianceAndMean().
45  * Other methods such as \a getMean() or \a getCovariance() are implemented here for convenience.
46  * \sa mprt::poses::CPosePDF, mprt::poses::CPose3DPDF, mprt::poses::CPointPDF
47  * \ingroup mrpt_base_grp
48  */
49  template <class TDATA, size_t STATE_LEN>
50  class CProbabilityDensityFunction
51  {
52  public:
53  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).
54  typedef TDATA type_value; //!< The type of the state the PDF represents
55 
56  /** Returns the mean, or mathematical expectation of the probability density distribution (PDF).
57  * \sa getCovarianceAndMean, getInformationMatrix
58  */
59  virtual void getMean(TDATA &mean_point) const = 0;
60 
61  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
62  * \sa getMean, getInformationMatrix
63  */
64  virtual void getCovarianceAndMean(CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &cov,TDATA &mean_point) const = 0;
65 
66  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
67  * \sa getMean, getInformationMatrix
68  */
69  inline void getCovarianceDynAndMean(CMatrixDouble &cov,TDATA &mean_point) const
70  {
71  CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> C(UNINITIALIZED_MATRIX);
72  this->getCovarianceAndMean(C,mean_point);
73  cov = C; // Convert to dynamic size matrix
74  }
75 
76  /** Returns the mean, or mathematical expectation of the probability density distribution (PDF).
77  * \sa getCovariance, getInformationMatrix
78  */
79  inline TDATA getMeanVal() const
80  {
81  TDATA p;
82  getMean(p);
83  return p;
84  }
85 
86  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
87  * \sa getMean, getCovarianceAndMean, getInformationMatrix
88  */
89  inline void getCovariance(CMatrixDouble &cov) const
90  {
91  TDATA p;
92  this->getCovarianceDynAndMean(cov,p);
93  }
94 
95  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
96  * \sa getMean, getCovarianceAndMean, getInformationMatrix
97  */
98  inline void getCovariance(CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &cov) const
99  {
100  TDATA p;
101  this->getCovarianceAndMean(cov,p);
102  }
103 
104  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
105  * \sa getMean, getInformationMatrix
106  */
107  inline CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> getCovariance() const
108  {
109  CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> cov(UNINITIALIZED_MATRIX);
110  TDATA p;
111  this->getCovarianceAndMean(cov,p);
112  return cov;
113  }
114 
115 
116  /** Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix)
117  * Unless reimplemented in derived classes, this method first reads the covariance, then invert it.
118  * \sa getMean, getCovarianceAndMean
119  */
120  virtual void getInformationMatrix(CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> &inf) const
121  {
122  CMatrixFixedNumeric<double,STATE_LEN,STATE_LEN> cov(UNINITIALIZED_MATRIX);
123  TDATA p;
124  this->getCovarianceAndMean(cov,p);
125  cov.inv_fast(inf); // Destroy source cov matrix, since we don't need it anymore.
126  }
127 
128  /** Save PDF's particles to a text file. See derived classes for more information about the format of generated files.
129  */
130  virtual void saveToTextFile(const std::string &file) const = 0;
131 
132  /** Draws a single sample from the distribution
133  */
134  virtual void drawSingleSample( TDATA &outPart ) const = 0;
135 
136  /** 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.
137  * This base method just call N times to drawSingleSample, but derived classes should implemented optimized method for each particular PDF.
138  */
139  virtual void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const
140  {
141  outSamples.resize(N);
142  TDATA pnt;
143  for (size_t i=0;i<N;i++)
144  {
145  this->drawSingleSample(pnt);
146  pnt.getAsVector(outSamples[i]);
147  }
148  }
149 
150  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
151  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
152  */
153  virtual void changeCoordinatesReference( const mrpt::poses::CPose3D &newReferenceBase ) = 0;
154 
155  /** Compute the entropy of the estimated covariance matrix.
156  * \sa http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy
157  */
158  inline double getCovarianceEntropy() const
159  {
160  static const double ln_2PI= 1.8378770664093454835606594728112;
161  return 0.5*( STATE_LEN + STATE_LEN * ln_2PI + log( std::max(getCovariance().det(), std::numeric_limits<double>::epsilon() ) ) );
162  }
163 
164  }; // End of class def.
165 
166  } // End of namespace
167 } // End of namespace
168 
169 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013