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-2017, 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
24  * here for convenience.
25  * \sa mprt::poses::CPosePDF, mprt::poses::CPose3DPDF, mprt::poses::CPointPDF
26  * \ingroup mrpt_base_grp
27  */
28 template <class TDATA, size_t STATE_LEN>
30 {
31  public:
32  /** The length of the variable, for example, 3 for a 3D point, 6 for a 3D
33  * pose (x y z yaw pitch roll). */
34  static const size_t state_length = STATE_LEN;
35  /** The type of the state the PDF represents */
36  typedef TDATA type_value;
38 
39  /** Returns the mean, or mathematical expectation of the probability density
40  * distribution (PDF).
41  * \sa getCovarianceAndMean, getInformationMatrix
42  */
43  virtual void getMean(TDATA& mean_point) const = 0;
44 
45  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN
46  * cov matrix) and the mean, both at once.
47  * \sa getMean, getInformationMatrix
48  */
49  virtual void getCovarianceAndMean(
51  TDATA& mean_point) const = 0;
52 
53  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN
54  * cov matrix) and the mean, both at once.
55  * \sa getMean, getInformationMatrix
56  */
58  mrpt::math::CMatrixDouble& cov, TDATA& mean_point) const
59  {
62  this->getCovarianceAndMean(C, mean_point);
63  cov = C; // Convert to dynamic size matrix
64  }
65 
66  /** Returns the mean, or mathematical expectation of the probability density
67  * distribution (PDF).
68  * \sa getCovariance, getInformationMatrix
69  */
70  inline TDATA getMeanVal() const
71  {
72  TDATA p;
73  getMean(p);
74  return p;
75  }
76 
77  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN
78  * covariance matrix)
79  * \sa getMean, getCovarianceAndMean, getInformationMatrix
80  */
82  {
83  TDATA p;
84  this->getCovarianceDynAndMean(cov, p);
85  }
86 
87  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN
88  * covariance matrix)
89  * \sa getMean, getCovarianceAndMean, getInformationMatrix
90  */
91  inline void getCovariance(
93  const
94  {
95  TDATA p;
96  this->getCovarianceAndMean(cov, p);
97  }
98 
99  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN
100  * covariance matrix)
101  * \sa getMean, getInformationMatrix
102  */
105  {
108  TDATA p;
109  this->getCovarianceAndMean(cov, p);
110  return cov;
111  }
112 
113  /** Returns whether the class instance holds the uncertainty in covariance
114  * or information form.
115  * \note By default this is going to be covariance form. *Inf classes
116  * (e.g. CPosePDFGaussianInf) store it in information form.
117  *
118  * \sa mrpt::traits::is_inf_type
119  */
120  virtual bool isInfType() const { return false; }
121  /** Returns the information (inverse covariance) matrix (a STATE_LEN x
122  * STATE_LEN matrix)
123  * Unless reimplemented in derived classes, this method first reads the
124  * covariance, then invert it.
125  * \sa getMean, getCovarianceAndMean
126  */
127  virtual void getInformationMatrix(
129  const
130  {
133  TDATA p;
134  this->getCovarianceAndMean(cov, p);
135  cov.inv_fast(
136  inf); // Destroy source cov matrix, since we don't need it anymore.
137  }
138 
139  /** Save PDF's particles to a text file. See derived classes for more
140  * information about the format of generated files.
141  */
142  virtual void 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  /** this = p (+) this. This can be used to convert a PDF from local
167  * coordinates to global, providing the point (newReferenceBase) from which
168  * "to project" the current pdf. Result PDF substituted the currently
169  * stored one in the object.
170  */
171  virtual void changeCoordinatesReference(
172  const mrpt::poses::CPose3D& newReferenceBase) = 0;
173 
174  /** Compute the entropy of the estimated covariance matrix.
175  * \sa
176  * http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy
177  */
178  inline double getCovarianceEntropy() const
179  {
180  static const double ln_2PI = 1.8378770664093454835606594728112;
181  return 0.5 * (STATE_LEN + STATE_LEN * ln_2PI +
182  log(std::max(
183  getCovariance().det(),
184  std::numeric_limits<double>::epsilon())));
185  }
186 
187 }; // End of class def.
188 
189 } // End of namespace
190 } // End of namespace
191 
192 #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)...
mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > getCovariance() const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
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...
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)...
GLsizei const GLchar ** string
Definition: glext.h:4101
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TDATA type_value
The type of the state the PDF represents.
CProbabilityDensityFunction< TDATA, STATE_LEN > self_t
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
virtual bool isInfType() const
Returns whether the class instance holds the uncertainty in covariance or information form...
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...
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
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).
GLfloat GLfloat p
Definition: glext.h:6305



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019