Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 #pragma once
10 
13 #include <mrpt/math/math_frwds.h>
14 
15 namespace mrpt
16 {
17 namespace math
18 {
19 /** A generic template for probability density distributions (PDFs).
20  * This template is used as base for many classes in mrpt::poses
21  * Any derived class must implement \a getMean() and a getCovarianceAndMean().
22  * Other methods such as \a getMean() or \a getCovariance() are implemented
23  * here for convenience.
24  * \sa mprt::poses::CPosePDF, mprt::poses::CPose3DPDF, mprt::poses::CPointPDF
25  * \ingroup mrpt_math_grp
26  */
27 template <class TDATA, size_t STATE_LEN>
29 {
30  public:
31  /** The length of the variable, for example, 3 for a 3D point, 6 for a 3D
32  * pose (x y z yaw pitch roll). */
33  static const size_t state_length = STATE_LEN;
34  /** The type of the state the PDF represents */
35  using type_value = TDATA;
37 
38  /** Returns the mean, or mathematical expectation of the probability density
39  * distribution (PDF).
40  * \sa getCovarianceAndMean, getInformationMatrix
41  */
42  virtual void getMean(TDATA& mean_point) const = 0;
43 
44  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN
45  * cov matrix) and the mean, both at once.
46  * \sa getMean, getInformationMatrix
47  */
48  virtual void getCovarianceAndMean(
50  TDATA& mean_point) const = 0;
51 
52  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN
53  * cov matrix) and the mean, both at once.
54  * \sa getMean, getInformationMatrix
55  */
57  mrpt::math::CMatrixDouble& cov, TDATA& mean_point) const
58  {
61  this->getCovarianceAndMean(C, mean_point);
62  cov = C; // Convert to dynamic size matrix
63  }
64 
65  /** Returns the mean, or mathematical expectation of the probability density
66  * distribution (PDF).
67  * \sa getCovariance, getInformationMatrix
68  */
69  inline TDATA getMeanVal() const
70  {
71  TDATA p;
72  getMean(p);
73  return p;
74  }
75 
76  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN
77  * covariance matrix)
78  * \sa getMean, getCovarianceAndMean, getInformationMatrix
79  */
81  {
82  TDATA p;
83  this->getCovarianceDynAndMean(cov, p);
84  }
85 
86  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN
87  * covariance matrix)
88  * \sa getMean, getCovarianceAndMean, getInformationMatrix
89  */
90  inline void getCovariance(
92  const
93  {
94  TDATA p;
95  this->getCovarianceAndMean(cov, p);
96  }
97 
98  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN
99  * covariance matrix)
100  * \sa getMean, getInformationMatrix
101  */
104  {
107  TDATA p;
108  this->getCovarianceAndMean(cov, p);
109  return cov;
110  }
111 
112  /** Returns whether the class instance holds the uncertainty in covariance
113  * or information form.
114  * \note By default this is going to be covariance form. *Inf classes
115  * (e.g. CPosePDFGaussianInf) store it in information form.
116  *
117  * \sa mrpt::traits::is_inf_type
118  */
119  virtual bool isInfType() const { return false; }
120  /** Returns the information (inverse covariance) matrix (a STATE_LEN x
121  * STATE_LEN matrix)
122  * Unless reimplemented in derived classes, this method first reads the
123  * covariance, then invert it.
124  * \sa getMean, getCovarianceAndMean
125  */
126  virtual void getInformationMatrix(
128  const
129  {
132  TDATA p;
133  this->getCovarianceAndMean(cov, p);
134  cov.inv_fast(
135  inf); // Destroy source cov matrix, since we don't need it anymore.
136  }
137 
138  /** Save PDF's particles to a text file. See derived classes for more
139  * information about the format of generated files.
140  * \return false on error
141  */
142  virtual bool saveToTextFile(const std::string& file) const = 0;
143 
144  /** Draws a single sample from the distribution
145  */
146  virtual void drawSingleSample(TDATA& outPart) const = 0;
147 
148  /** Draws a number of samples from the distribution, and saves as a list of
149  * 1xSTATE_LEN vectors, where each row contains a (x,y,z,yaw,pitch,roll)
150  * datum.
151  * This base method just call N times to drawSingleSample, but derived
152  * classes should implemented optimized method for each particular PDF.
153  */
154  virtual void drawManySamples(
155  size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const
156  {
157  outSamples.resize(N);
158  TDATA pnt;
159  for (size_t i = 0; i < N; i++)
160  {
161  this->drawSingleSample(pnt);
162  pnt.getAsVector(outSamples[i]);
163  }
164  }
165 
166  /** Compute the entropy of the estimated covariance matrix.
167  * \sa
168  * http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy
169  */
170  double getCovarianceEntropy() const
171  {
172  static const double ln_2PI = 1.8378770664093454835606594728112;
173  return 0.5 * (STATE_LEN + STATE_LEN * ln_2PI +
174  log(std::max(
175  getCovariance().det(),
176  std::numeric_limits<double>::epsilon())));
177  }
178 
179 }; // End of class def.
180 
181 } // End of namespace
182 } // End of namespace
mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > getCovariance() const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
EIGEN_STRONG_INLINE Scalar det() const
virtual bool isInfType() const
Returns whether the class instance holds the uncertainty in covariance or information form...
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) ...
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...
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 getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
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...
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
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...
A numeric matrix of compile-time fixed size.
GLsizei const GLchar ** string
Definition: glext.h:4101
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)...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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.
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:148
virtual bool saveToTextFile(const std::string &file) const =0
Save PDF&#39;s particles to a text file.
GLfloat GLfloat p
Definition: glext.h:6305
A generic template for probability density distributions (PDFs).
CPose2D type_value
The type of the state the PDF represents.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019