Main MRPT website > C++ reference for MRPT 1.5.6
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
TDATA getMeanVal() const
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 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 drawSingleSample(TDATA &outPart) const =0
Draws a single sample from the distribution.
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.
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
double getCovarianceEntropy() const
Compute the entropy of the estimated covariance matrix.
GLfloat GLfloat p
Definition: glew.h:10113
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)...
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) ...
mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > getCovariance() const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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 void saveToTextFile(const std::string &file) const =0
Save PDF'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...
virtual bool isInfType() const
Returns whether the class instance holds the uncertainty in covariance or information form...
A generic template for probability density distributions (PDFs).
EIGEN_STRONG_INLINE Scalar det() const
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...



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018