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
mrpt::utils::CProbabilityDensityFunction::drawSingleSample
virtual void drawSingleSample(TDATA &outPart) const =0
Draws a single sample from the distribution.
mrpt::utils::CProbabilityDensityFunction::changeCoordinatesReference
virtual void changeCoordinatesReference(const mrpt::poses::CPose3D &newReferenceBase)=0
this = p (+) this.
CMatrixFixedNumeric.h
mrpt::utils::CProbabilityDensityFunction::getCovariance
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
Definition: CProbabilityDensityFunction.h:67
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::utils::CProbabilityDensityFunction::getMean
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
mrpt::math::CMatrixTemplateNumeric< double >
mrpt::utils::CProbabilityDensityFunction
A generic template for probability density distributions (PDFs).
Definition: CProbabilityDensityFunction.h:28
CMatrixTemplateNumeric.h
mrpt::utils::CProbabilityDensityFunction::getCovarianceEntropy
double getCovarianceEntropy() const
Compute the entropy of the estimated covariance matrix.
Definition: CProbabilityDensityFunction.h:136
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
math_frwds.h
mrpt::utils::CProbabilityDensityFunction::type_value
TDATA type_value
The type of the state the PDF represents.
Definition: CProbabilityDensityFunction.h:32
mrpt::utils::CProbabilityDensityFunction::getCovarianceAndMean
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,...
mrpt::utils::CProbabilityDensityFunction::getCovariance
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)
Definition: CProbabilityDensityFunction.h:76
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:34
mrpt::utils::CProbabilityDensityFunction::getCovariance
mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > getCovariance() const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
Definition: CProbabilityDensityFunction.h:85
mrpt::utils::CProbabilityDensityFunction::getCovarianceDynAndMean
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,...
Definition: CProbabilityDensityFunction.h:47
mrpt::math::UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
mrpt::utils::CProbabilityDensityFunction::state_length
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).
Definition: CProbabilityDensityFunction.h:31
mrpt::utils::CProbabilityDensityFunction::getInformationMatrix
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...
Definition: CProbabilityDensityFunction.h:98
det
EIGEN_STRONG_INLINE Scalar det() const
Definition: eigen_plugins.h:475
mrpt::math::cov
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
mrpt::utils::CProbabilityDensityFunction::getMeanVal
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
Definition: CProbabilityDensityFunction.h:57
mrpt::utils::CProbabilityDensityFunction::saveToTextFile
virtual void saveToTextFile(const std::string &file) const =0
Save PDF's particles to a text file.
mrpt::utils::CProbabilityDensityFunction::drawManySamples
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,...
Definition: CProbabilityDensityFunction.h:117



Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 22:32:58 UTC 2019