MRPT  2.0.1
CPointPDFSOG.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 
11 #include <mrpt/math/CMatrixD.h>
12 #include <mrpt/math/CMatrixF.h>
13 #include <mrpt/poses/CPointPDF.h>
15 
16 namespace mrpt::poses
17 {
18 /** Declares a class that represents a Probability Density function (PDF) of a
19  * 3D point \f$ p(\mathbf{x}) = [x ~ y ~ z ]^t \f$.
20  * This class implements that PDF as the following multi-modal Gaussian
21  * distribution:
22  *
23  * \f$ p(\mathbf{x}) = \sum\limits_{i=1}^N \omega^i \mathcal{N}( \mathbf{x} ;
24  * \bar{\mathbf{x}}^i, \mathbf{\Sigma}^i ) \f$
25  *
26  * Where the number of modes N is the size of CPointPDFSOG::m_modes
27  *
28  * See mrpt::poses::CPointPDF for more details.
29  *
30  * \sa CPointPDF, CPosePDF,
31  * \ingroup poses_pdf_grp
32  */
33 class CPointPDFSOG : public CPointPDF
34 {
36 
37  public:
38  /** The struct for each mode:
39  */
41  {
42  TGaussianMode() : val() {}
44 
45  /** The log-weight
46  */
47  double log_w{0};
48  };
49 
50  using CListGaussianModes = std::deque<TGaussianMode>;
51  using const_iterator = std::deque<TGaussianMode>::const_iterator;
52  using iterator = std::deque<TGaussianMode>::iterator;
53 
54  protected:
55  /** Assures the symmetry of the covariance matrix (eventually certain
56  * operations in the math-coprocessor lead to non-symmetric matrixes!)
57  */
58  void enforceCovSymmetry();
59 
60  /** The list of SOG modes */
62 
63  public:
64  /** Default constructor
65  * \param nModes The initial size of CPointPDFSOG::m_modes
66  */
67  CPointPDFSOG(size_t nModes = 1);
68 
69  /** Clear all the gaussian modes */
70  void clear();
71 
72  /** Access to individual beacons */
73  const TGaussianMode& operator[](size_t i) const
74  {
75  ASSERT_(i < m_modes.size());
76  return m_modes[i];
77  }
78  /** Access to individual beacons */
80  {
81  ASSERT_(i < m_modes.size());
82  return m_modes[i];
83  }
84 
85  /** Access to individual beacons */
86  const TGaussianMode& get(size_t i) const
87  {
88  ASSERT_(i < m_modes.size());
89  return m_modes[i];
90  }
91  /** Access to individual beacons */
92  TGaussianMode& get(size_t i)
93  {
94  ASSERT_(i < m_modes.size());
95  return m_modes[i];
96  }
97 
98  /** Inserts a copy of the given mode into the SOG */
99  void push_back(const TGaussianMode& m) { m_modes.push_back(m); }
100  iterator begin() { return m_modes.begin(); }
101  iterator end() { return m_modes.end(); }
102  const_iterator begin() const { return m_modes.begin(); }
103  const_iterator end() const { return m_modes.end(); }
104  iterator erase(iterator i) { return m_modes.erase(i); }
105  /** Resize the number of SOG modes */
106  void resize(const size_t N);
107  /** Return the number of Gaussian modes. */
108  size_t size() const { return m_modes.size(); }
109  /** Return whether there is any Gaussian mode. */
110  bool empty() const { return m_modes.empty(); }
111 
112  void getMean(CPoint3D& mean_point) const override;
113 
114  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override;
115 
116  /** Normalize the weights in m_modes such as the maximum log-weight is 0 */
117  void normalizeWeights();
118 
119  /** Return the Gaussian mode with the highest likelihood (or an empty
120  * Gaussian if there are no modes in this SOG) */
121  void getMostLikelyMode(CPointPDFGaussian& outVal) const;
122 
123  /** Computes the "Effective sample size" (typical measure for Particle
124  * Filters), applied to the weights of the individual Gaussian modes, as a
125  * measure of the equality of the modes (in the range [0,total # of modes]).
126  */
127  double ESS() const;
128 
129  /** Copy operator, translating if necesary (for example, between particles
130  * and gaussian representations) */
131  void copyFrom(const CPointPDF& o) override;
132 
133  /** Save the density to a text file, with the following format:
134  * There is one row per Gaussian "mode", and each row contains 10
135  * elements:
136  * - w (The weight)
137  * - x_mean (gaussian mean value)
138  * - y_mean (gaussian mean value)
139  * - x_mean (gaussian mean value)
140  * - C11 (Covariance elements)
141  * - C22 (Covariance elements)
142  * - C33 (Covariance elements)
143  * - C12 (Covariance elements)
144  * - C13 (Covariance elements)
145  * - C23 (Covariance elements)
146  *
147  */
148  bool saveToTextFile(const std::string& file) const override;
149 
150  /** this = p (+) this. This can be used to convert a PDF from local
151  * coordinates to global, providing the point (newReferenceBase) from which
152  * "to project" the current pdf. Result PDF substituted the currently
153  * stored one in the object. */
154  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
155 
156  /** Draw a sample from the pdf. */
157  void drawSingleSample(CPoint3D& outSample) const override;
158 
159  /** Bayesian fusion of two point distributions (product of two
160  * distributions->new distribution), then save the result in this object
161  * (WARNING: See implementing classes to see classes that can and cannot be
162  * mixtured!)
163  * \param p1 The first distribution to fuse
164  * \param p2 The second distribution to fuse
165  * \param minMahalanobisDistToDrop If set to different of 0, the result of
166  * very separate Gaussian modes (that will result in negligible components)
167  * in SOGs will be dropped to reduce the number of modes in the output.
168  */
169  void bayesianFusion(
170  const CPointPDF& p1, const CPointPDF& p2,
171  const double minMahalanobisDistToDrop = 0) override;
172 
173  /** Evaluates the PDF within a rectangular grid and saves the result in a
174  * matrix (each row contains values for a fixed y-coordinate value).
175  */
176  void evaluatePDFInArea(
177  float x_min, float x_max, float y_min, float y_max, float resolutionXY,
178  float z, mrpt::math::CMatrixD& outMatrix, bool sumOverAllZs = false);
179 
180  /** Evaluates the PDF at a given point */
181  double evaluatePDF(const CPoint3D& x, bool sumOverAllZs) const;
182 
183 }; // End of class def.
184 } // namespace mrpt::poses
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
void getMean(CPoint3D &mean_point) const override
void resize(const size_t N)
Resize the number of SOG modes.
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:33
The struct for each mode:
Definition: CPointPDFSOG.h:40
const TGaussianMode & operator[](size_t i) const
Access to individual beacons.
Definition: CPointPDFSOG.h:73
void clear()
Clear all the gaussian modes.
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
bool empty() const
Return whether there is any Gaussian mode.
Definition: CPointPDFSOG.h:110
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
Definition: CPointPDFSOG.h:99
TGaussianMode & operator[](size_t i)
Access to individual beacons.
Definition: CPointPDFSOG.h:79
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
iterator erase(iterator i)
Definition: CPointPDFSOG.h:104
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
std::deque< TGaussianMode >::const_iterator const_iterator
Definition: CPointPDFSOG.h:51
double ESS() const
Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights o...
size_t size() const
Return the number of Gaussian modes.
Definition: CPointPDFSOG.h:108
const_iterator begin() const
Definition: CPointPDFSOG.h:102
const_iterator end() const
Definition: CPointPDFSOG.h:103
void evaluatePDFInArea(float x_min, float x_max, float y_min, float y_max, float resolutionXY, float z, mrpt::math::CMatrixD &outMatrix, bool sumOverAllZs=false)
Evaluates the PDF within a rectangular grid and saves the result in a matrix (each row contains value...
CPointPDFSOG(size_t nModes=1)
Default constructor.
void getMostLikelyMode(CPointPDFGaussian &outVal) const
Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in t...
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:36
double evaluatePDF(const CPoint3D &x, bool sumOverAllZs) const
Evaluates the PDF at a given point.
std::deque< TGaussianMode > CListGaussianModes
Definition: CPointPDFSOG.h:50
CListGaussianModes m_modes
The list of SOG modes.
Definition: CPointPDFSOG.h:61
bool saveToTextFile(const std::string &file) const override
Save the density to a text file, with the following format: There is one row per Gaussian "mode"...
A gaussian distribution for 3D points.
void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
std::deque< TGaussianMode >::iterator iterator
Definition: CPointPDFSOG.h:52



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020