Main MRPT website > C++ reference for MRPT 1.4.0
CProbabilityDensityFunction.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CProbabilityDensityFunction_H
10 #define CProbabilityDensityFunction_H
11 
14 #include <mrpt/math/math_frwds.h>
15 
16 namespace mrpt
17 {
18  namespace utils
19  {
20  /** A generic template for probability density distributions (PDFs).
21  * This template is used as base for many classes in mrpt::poses
22  * Any derived class must implement \a getMean() and a getCovarianceAndMean().
23  * Other methods such as \a getMean() or \a getCovariance() are implemented here for convenience.
24  * \sa mprt::poses::CPosePDF, mprt::poses::CPose3DPDF, mprt::poses::CPointPDF
25  * \ingroup mrpt_base_grp
26  */
27  template <class TDATA, size_t STATE_LEN>
29  {
30  public:
31  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).
32  typedef TDATA type_value; //!< The type of the state the PDF represents
33 
34  /** Returns the mean, or mathematical expectation of the probability density distribution (PDF).
35  * \sa getCovarianceAndMean, getInformationMatrix
36  */
37  virtual void getMean(TDATA &mean_point) const = 0;
38 
39  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
40  * \sa getMean, getInformationMatrix
41  */
43 
44  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
45  * \sa getMean, getInformationMatrix
46  */
47  inline void getCovarianceDynAndMean(mrpt::math::CMatrixDouble &cov,TDATA &mean_point) const
48  {
50  this->getCovarianceAndMean(C,mean_point);
51  cov = C; // Convert to dynamic size matrix
52  }
53 
54  /** Returns the mean, or mathematical expectation of the probability density distribution (PDF).
55  * \sa getCovariance, getInformationMatrix
56  */
57  inline TDATA getMeanVal() const
58  {
59  TDATA p;
60  getMean(p);
61  return p;
62  }
63 
64  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
65  * \sa getMean, getCovarianceAndMean, getInformationMatrix
66  */
68  {
69  TDATA p;
70  this->getCovarianceDynAndMean(cov,p);
71  }
72 
73  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
74  * \sa getMean, getCovarianceAndMean, getInformationMatrix
75  */
77  {
78  TDATA p;
79  this->getCovarianceAndMean(cov,p);
80  }
81 
82  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
83  * \sa getMean, getInformationMatrix
84  */
86  {
88  TDATA p;
89  this->getCovarianceAndMean(cov,p);
90  return cov;
91  }
92 
93 
94  /** Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix)
95  * Unless reimplemented in derived classes, this method first reads the covariance, then invert it.
96  * \sa getMean, getCovarianceAndMean
97  */
99  {
101  TDATA p;
102  this->getCovarianceAndMean(cov,p);
103  cov.inv_fast(inf); // Destroy source cov matrix, since we don't need it anymore.
104  }
105 
106  /** Save PDF's particles to a text file. See derived classes for more information about the format of generated files.
107  */
108  virtual void saveToTextFile(const std::string &file) const = 0;
109 
110  /** Draws a single sample from the distribution
111  */
112  virtual void drawSingleSample( TDATA &outPart ) const = 0;
113 
114  /** 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.
115  * This base method just call N times to drawSingleSample, but derived classes should implemented optimized method for each particular PDF.
116  */
117  virtual void drawManySamples( size_t N, std::vector<mrpt::math::CVectorDouble> & outSamples ) const
118  {
119  outSamples.resize(N);
120  TDATA pnt;
121  for (size_t i=0;i<N;i++)
122  {
123  this->drawSingleSample(pnt);
124  pnt.getAsVector(outSamples[i]);
125  }
126  }
127 
128  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
129  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
130  */
131  virtual void changeCoordinatesReference( const mrpt::poses::CPose3D &newReferenceBase ) = 0;
132 
133  /** Compute the entropy of the estimated covariance matrix.
134  * \sa http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy
135  */
136  inline double getCovarianceEntropy() const
137  {
138  static const double ln_2PI= 1.8378770664093454835606594728112;
139  return 0.5*( STATE_LEN + STATE_LEN * ln_2PI + log( std::max(getCovariance().det(), std::numeric_limits<double>::epsilon() ) ) );
140  }
141 
142  }; // End of class def.
143 
144  } // End of namespace
145 } // End of namespace
146 
147 #endif
EIGEN_STRONG_INLINE Scalar det() const
virtual void drawSingleSample(TDATA &outPart) const =0
Draws a single sample from the distribution.
double getCovarianceEntropy() const
Compute the entropy of the estimated covariance matrix.
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
A numeric matrix of compile-time fixed size.
void getCovarianceDynAndMean(mrpt::math::CMatrixDouble &cov, TDATA &mean_point) const
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:135
virtual void getInformationMatrix(mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &inf) const
Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix) Unless reimpleme...
virtual void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const
Draws a number of samples from the distribution, and saves as a list of 1xSTATE_LEN vectors...
virtual void changeCoordinatesReference(const mrpt::poses::CPose3D &newReferenceBase)=0
this = p (+) this.
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > getCovariance() const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
TDATA type_value
The type of the state the PDF represents.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
virtual void saveToTextFile(const std::string &file) const =0
Save PDF&#39;s particles to a text file.
static const size_t state_length
The length of the variable, for example, 3 for a 3D point, 6 for a 3D pose (x y z yaw pitch roll)...
virtual void getCovarianceAndMean(mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov, TDATA &mean_point) const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
void getCovariance(mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
A generic template for probability density distributions (PDFs).



Page generated by Doxygen 1.8.13 for MRPT 1.4.0 SVN: at Fri Mar 17 07:27:15 UTC 2017