MRPT  2.0.4
CProbabilityDensityFunction.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
12 #include <mrpt/math/CMatrixFixed.h>
14 #include <mrpt/math/math_frwds.h>
15 
16 namespace mrpt::math
17 {
18 /** A generic template for probability density distributions (PDFs).
19  * This template is used as base for many classes in mrpt::poses
20  * Any derived class must implement \a getMean() and a getCovarianceAndMean().
21  * Other methods such as \a getMean() or \a getCovariance() are implemented
22  * here for convenience.
23  * \sa mprt::poses::CPosePDF, mprt::poses::CPose3DPDF, mprt::poses::CPointPDF
24  * \ingroup mrpt_math_grp
25  */
26 template <class TDATA, size_t STATE_LEN>
28 {
29  public:
30  /** The length of the variable, for example, 3 for a 3D point, 6 for a 3D
31  * pose (x y z yaw pitch roll). */
32  static constexpr size_t state_length = STATE_LEN;
33  /** The type of the state the PDF represents */
34  using type_value = TDATA;
36  /** Covariance matrix type */
38  /** Information matrix type */
40 
41  /** Returns the mean, or mathematical expectation of the probability density
42  * distribution (PDF).
43  * \sa getCovarianceAndMean, getInformationMatrix
44  */
45  virtual void getMean(type_value& mean_point) const = 0;
46 
47  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN
48  * cov matrix) and the mean, both at once.
49  * \sa getMean, getInformationMatrix
50  */
51  virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const = 0;
52 
53  /// \overload
54  virtual void getCovarianceAndMean(cov_mat_t& c, TDATA& mean) const final
55  {
56  const auto [C, M] = getCovarianceAndMean();
57  c = C;
58  mean = M;
59  }
60 
61  /** Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN
62  * cov matrix) and the mean, both at once.
63  * \sa getMean, getInformationMatrix
64  */
66  mrpt::math::CMatrixDouble& cov, type_value& mean_point) const
67  {
69  this->getCovarianceAndMean(C, mean_point);
70  cov = C; // Convert to dynamic size matrix
71  }
72 
73  /** Returns the mean, or mathematical expectation of the probability density
74  * distribution (PDF).
75  * \sa getCovariance, getInformationMatrix
76  */
77  inline type_value getMeanVal() const
78  {
79  type_value p;
80  getMean(p);
81  return p;
82  }
83 
84  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN
85  * covariance matrix)
86  * \sa getMean, getCovarianceAndMean, getInformationMatrix
87  */
89  {
90  TDATA p;
91  this->getCovarianceDynAndMean(cov, p);
92  }
93 
94  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN
95  * covariance matrix)
96  * \sa getMean, getCovarianceAndMean, getInformationMatrix
97  */
98  inline void getCovariance(cov_mat_t& cov) const
99  {
100  TDATA p;
101  this->getCovarianceAndMean(cov, p);
102  }
103 
104  /** Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN
105  * covariance matrix)
106  * \sa getMean, getInformationMatrix
107  */
108  inline cov_mat_t getCovariance() const
109  {
111  TDATA p;
112  this->getCovarianceAndMean(cov, p);
113  return cov;
114  }
115 
116  /** Returns whether the class instance holds the uncertainty in covariance
117  * or information form.
118  * \note By default this is going to be covariance form. *Inf classes
119  * (e.g. CPosePDFGaussianInf) store it in information form.
120  *
121  * \sa mrpt::traits::is_inf_type
122  */
123  virtual bool isInfType() const { return false; }
124  /** Returns the information (inverse covariance) matrix (a STATE_LEN x
125  * STATE_LEN matrix)
126  * Unless reimplemented in derived classes, this method first reads the
127  * covariance, then invert it.
128  * \sa getMean, getCovarianceAndMean
129  */
130  virtual void getInformationMatrix(inf_mat_t& inf) const
131  {
132  inf = getCovariance().inverse_LLt();
133  }
134 
135  /** Save PDF's particles to a text file. See derived classes for more
136  * information about the format of generated files.
137  * \return false on error
138  */
139  virtual bool saveToTextFile(const std::string& file) const = 0;
140 
141  /** Draws a single sample from the distribution
142  */
143  virtual void drawSingleSample(TDATA& outPart) const = 0;
144 
145  /** Draws a number of samples from the distribution, and saves as a list of
146  * 1xSTATE_LEN vectors, where each row contains a (x,y,z,yaw,pitch,roll)
147  * datum.
148  * This base method just call N times to drawSingleSample, but derived
149  * classes should implemented optimized method for each particular PDF.
150  */
151  virtual void drawManySamples(
152  size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const
153  {
154  outSamples.resize(N);
155  TDATA pnt;
156  for (size_t i = 0; i < N; i++)
157  {
158  this->drawSingleSample(pnt);
159  outSamples[i] = mrpt::math::CVectorDouble(pnt.asVectorVal());
160  }
161  }
162 
163  /** Compute the entropy of the estimated covariance matrix.
164  * \sa
165  * http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy
166  */
167  double getCovarianceEntropy() const
168  {
169  static const double ln_2PI = 1.8378770664093454835606594728112;
170  return 0.5 * (STATE_LEN + STATE_LEN * ln_2PI +
171  log(std::max(
172  getCovariance().det(),
173  std::numeric_limits<double>::epsilon())));
174  }
175 
176 }; // End of class def.
177 
178 } // namespace mrpt::math
cov_mat_t getCovariance() const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
virtual bool isInfType() const
Returns whether the class instance holds the uncertainty in covariance or information form...
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...
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
virtual void getMean(type_value &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
This base provides a set of functions for maths stuff.
CVectorDynamic< double > CVectorDouble
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
CMatrixDouble 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:149
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
static constexpr 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)...
void getCovarianceDynAndMean(mrpt::math::CMatrixDouble &cov, type_value &mean_point) const
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
virtual std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
virtual void getInformationMatrix(inf_mat_t &inf) const
Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix) Unless reimpleme...
virtual void drawSingleSample(TDATA &outPart) const =0
Draws a single sample from the distribution.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
double getCovarianceEntropy() const
Compute the entropy of the estimated covariance matrix.
void getCovariance(cov_mat_t &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
virtual bool saveToTextFile(const std::string &file) const =0
Save PDF&#39;s particles to a text file.
A generic template for probability density distributions (PDFs).
mrpt::math::CMatrixFixed< double, STATE_LEN, STATE_LEN > cov_mat_t
Covariance matrix type.
virtual void getCovarianceAndMean(cov_mat_t &c, TDATA &mean) const final
CPose2D type_value
The type of the state the PDF represents.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020