Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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 
12 #include <mrpt/poses/CPointPDF.h>
14 #include <mrpt/math/CMatrix.h>
15 #include <mrpt/math/CMatrixD.h>
16 
17 namespace mrpt
18 {
19  namespace poses
20  {
21  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointPDFSOG, CPointPDF )
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;
59 
60  protected:
61  /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)
62  */
63  void assureSymmetry();
64 
65  /** The list of SOG modes */
66  CListGaussianModes m_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).
115  * \sa getCovariance
116  */
117  void getMean(CPoint3D &mean_point) const;
118 
119  /** Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once.
120  * \sa getMean
121  */
122  void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov,CPoint3D &mean_point) const;
123 
124  /** Normalize the weights in m_modes such as the maximum log-weight is 0.
125  */
126  void normalizeWeights();
127 
128  /** Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG) */
129  void getMostLikelyMode( CPointPDFGaussian& outVal ) const;
130 
131  /** 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]).
132  */
133  double ESS() const;
134 
135  /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
136  */
137  void copyFrom(const CPointPDF &o);
138 
139  /** Save the density to a text file, with the following format:
140  * There is one row per Gaussian "mode", and each row contains 10 elements:
141  * - w (The weight)
142  * - x_mean (gaussian mean value)
143  * - y_mean (gaussian mean value)
144  * - x_mean (gaussian mean value)
145  * - C11 (Covariance elements)
146  * - C22 (Covariance elements)
147  * - C33 (Covariance elements)
148  * - C12 (Covariance elements)
149  * - C13 (Covariance elements)
150  * - C23 (Covariance elements)
151  *
152  */
153  void saveToTextFile(const std::string &file) const;
154 
155  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
156  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
157  */
158  void changeCoordinatesReference(const CPose3D &newReferenceBase );
159 
160  /** Draw a sample from the pdf.
161  */
162  void drawSingleSample(CPoint3D &outSample) const;
163 
164  /** 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!)
165  * \param p1 The first distribution to fuse
166  * \param p2 The second distribution to fuse
167  * \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.
168  */
169  void bayesianFusion( const CPointPDF &p1, const CPointPDF &p2, const double &minMahalanobisDistToDrop = 0);
170 
171 
172  /** Evaluates the PDF within a rectangular grid and saves the result in a matrix (each row contains values for a fixed y-coordinate value).
173  */
174  void evaluatePDFInArea(
175  float x_min,
176  float x_max,
177  float y_min,
178  float y_max,
179  float resolutionXY,
180  float z,
181  mrpt::math::CMatrixD &outMatrix,
182  bool sumOverAllZs = false );
183 
184  /** Evaluates the PDF at a given point.
185  */
186  double evaluatePDF(
187  const CPoint3D &x,
188  bool sumOverAllZs ) const;
189 
190 
191  }; // End of class def.
193 
194 
195  } // End of namespace
196 } // End of namespace
197 
198 #endif
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:30
std::deque< TGaussianMode > CListGaussianModes
Definition: CPointPDFSOG.h:56
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:35
The struct for each mode:
Definition: CPointPDFSOG.h:43
Scalar * iterator
Definition: eigen_plugins.h:23
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
const Scalar * const_iterator
Definition: eigen_plugins.h:24
bool empty() const
Return whether there is any Gaussian mode.
Definition: CPointPDFSOG.h:112
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
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
Definition: CPointPDFSOG.h:99
std::deque< TGaussianMode >::const_iterator const_iterator
Definition: CPointPDFSOG.h:57
#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 class used to store a 3D point.
Definition: CPoint3D.h:32
iterator erase(iterator i)
Definition: CPointPDFSOG.h:108
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
size_t size() const
Return the number of Gaussian modes.
Definition: CPointPDFSOG.h:111
const_iterator begin() const
Definition: CPointPDFSOG.h:105
const_iterator end() const
Definition: CPointPDFSOG.h:106
#define ASSERT_(f)
std::deque< TGaussianMode >::iterator iterator
Definition: CPointPDFSOG.h:58
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:38
CListGaussianModes m_modes
The list of SOG modes.
Definition: CPointPDFSOG.h:66
A gaussian distribution for 3D points.



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Mon Oct 3 19:22:36 UTC 2016