Main MRPT website > C++ reference for MRPT 1.5.7
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 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
34 
35  /** Returns the mean, or mathematical expectation of the probability density distribution (PDF).
36  * \sa getCovarianceAndMean, getInformationMatrix
37  */
38  virtual void getMean(TDATA &mean_point) const = 0;
39 
40  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
41  * \sa getMean, getInformationMatrix
42  */
44 
45  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
46  * \sa getMean, getInformationMatrix
47  */
48  inline void getCovarianceDynAndMean(mrpt::math::CMatrixDouble &cov,TDATA &mean_point) const
49  {
51  this->getCovarianceAndMean(C,mean_point);
52  cov = C; // Convert to dynamic size matrix
53  }
54 
55  /** Returns the mean, or mathematical expectation of the probability density distribution (PDF).
56  * \sa getCovariance, getInformationMatrix
57  */
58  inline TDATA getMeanVal() const
59  {
60  TDATA p;
61  getMean(p);
62  return p;
63  }
64 
65  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
66  * \sa getMean, getCovarianceAndMean, getInformationMatrix
67  */
69  {
70  TDATA p;
71  this->getCovarianceDynAndMean(cov,p);
72  }
73 
74  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
75  * \sa getMean, getCovarianceAndMean, getInformationMatrix
76  */
78  {
79  TDATA p;
80  this->getCovarianceAndMean(cov,p);
81  }
82 
83  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
84  * \sa getMean, getInformationMatrix
85  */
87  {
89  TDATA p;
90  this->getCovarianceAndMean(cov,p);
91  return cov;
92  }
93 
94  /** Returns whether the class instance holds the uncertainty in covariance or information form.
95  * \note By default this is going to be covariance form. *Inf classes
96  * (e.g. CPosePDFGaussianInf) store it in information form.
97  *
98  * \sa mrpt::traits::is_inf_type
99  */
100  virtual bool isInfType() const { return false; }
101 
102  /** Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix)
103  * Unless reimplemented in derived classes, this method first reads the covariance, then invert it.
104  * \sa getMean, getCovarianceAndMean
105  */
107  {
109  TDATA p;
110  this->getCovarianceAndMean(cov,p);
111  cov.inv_fast(inf); // Destroy source cov matrix, since we don't need it anymore.
112  }
113 
114  /** Save PDF's particles to a text file. See derived classes for more information about the format of generated files.
115  */
116  virtual void saveToTextFile(const std::string &file) const = 0;
117 
118  /** Draws a single sample from the distribution
119  */
120  virtual void drawSingleSample( TDATA &outPart ) const = 0;
121 
122  /** 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.
123  * This base method just call N times to drawSingleSample, but derived classes should implemented optimized method for each particular PDF.
124  */
125  virtual void drawManySamples( size_t N, std::vector<mrpt::math::CVectorDouble> & outSamples ) const
126  {
127  outSamples.resize(N);
128  TDATA pnt;
129  for (size_t i=0;i<N;i++)
130  {
131  this->drawSingleSample(pnt);
132  pnt.getAsVector(outSamples[i]);
133  }
134  }
135 
136  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
137  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
138  */
139  virtual void changeCoordinatesReference( const mrpt::poses::CPose3D &newReferenceBase ) = 0;
140 
141  /** Compute the entropy of the estimated covariance matrix.
142  * \sa http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy
143  */
144  inline double getCovarianceEntropy() const
145  {
146  static const double ln_2PI= 1.8378770664093454835606594728112;
147  return 0.5*( STATE_LEN + STATE_LEN * ln_2PI + log( std::max(getCovariance().det(), std::numeric_limits<double>::epsilon() ) ) );
148  }
149 
150  }; // End of class def.
151 
152  } // End of namespace
153 } // End of namespace
154 
155 #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)...
GLsizei const GLchar ** string
Definition: glext.h:3919
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.
CProbabilityDensityFunction< TDATA, STATE_LEN > self_t
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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...
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:5587



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019