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
15
16namespace 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
A numeric matrix of compile-time fixed size.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
A generic template for probability density distributions (PDFs).
void getCovariance(mrpt::math::CMatrixDouble &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,...
double getCovarianceEntropy() const
Compute the entropy of the estimated covariance matrix.
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
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).
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 getMeanVal() const
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)
virtual void changeCoordinatesReference(const mrpt::poses::CPose3D &newReferenceBase)=0
this = p (+) this.
virtual void drawSingleSample(TDATA &outPart) const =0
Draws a single sample from the distribution.
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,...
TDATA type_value
The type of the state the PDF represents.
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 saveToTextFile(const std::string &file) const =0
Save PDF's particles to a text file.
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,...
EIGEN_STRONG_INLINE Scalar det() const
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Thu Mar 23 03:22:58 UTC 2023