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



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 02:56:26 UTC 2022