Main MRPT website > C++ reference
MRPT logo
CPointPDFSOG.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPointPDFSOG_H
29 #define CPointPDFSOG_H
30 
31 #include <mrpt/poses/CPointPDF.h>
32 #include <mrpt/poses/CPointPDFGaussian.h>
33 #include <mrpt/math/CMatrix.h>
34 #include <mrpt/math/CMatrixD.h>
35 
36 namespace mrpt
37 {
38  namespace poses
39  {
40  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointPDFSOG, CPointPDF )
41 
42  /** Declares a class that represents a Probability Density function (PDF) of a 3D point \f$ p(\mathbf{x}) = [x ~ y ~ z ]^t \f$.
43  * This class implements that PDF as the following multi-modal Gaussian distribution:
44  *
45  * \f$ p(\mathbf{x}) = \sum\limits_{i=1}^N \omega^i \mathcal{N}( \mathbf{x} ; \bar{\mathbf{x}}^i, \mathbf{\Sigma}^i ) \f$
46  *
47  * Where the number of modes N is the size of CPointPDFSOG::m_modes
48  *
49  * See mrpt::poses::CPointPDF for more details.
50  *
51  * \sa CPointPDF, CPosePDF,
52  * \ingroup poses_pdf_grp
53  */
54  class BASE_IMPEXP CPointPDFSOG : public CPointPDF
55  {
56  // This must be added to any CSerializable derived class:
57  DEFINE_SERIALIZABLE( CPointPDFSOG )
58 
59  public:
60  /** The struct for each mode:
61  */
62  struct BASE_IMPEXP TGaussianMode
63  {
64  TGaussianMode() : val(), log_w(0)
65  {
66  }
67 
68  CPointPDFGaussian val;
69 
70  /** The log-weight
71  */
72  double log_w;
73  };
74 
75  typedef std::deque<TGaussianMode> CListGaussianModes;
76  typedef std::deque<TGaussianMode>::const_iterator const_iterator;
77  typedef std::deque<TGaussianMode>::iterator iterator;
78 
79  protected:
80  /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)
81  */
82  void assureSymmetry();
83 
84  /** The list of SOG modes */
85  CListGaussianModes m_modes;
86 
87  public:
88  /** Default constructor
89  * \param nModes The initial size of CPointPDFSOG::m_modes
90  */
91  CPointPDFSOG( size_t nModes = 1 );
92 
93  void clear(); //!< Clear all the gaussian modes
94 
95  /** Access to individual beacons */
96  const TGaussianMode& operator [](size_t i) const {
97  ASSERT_(i<m_modes.size())
98  return m_modes[i];
99  }
100  /** Access to individual beacons */
101  TGaussianMode& operator [](size_t i) {
102  ASSERT_(i<m_modes.size())
103  return m_modes[i];
104  }
105 
106  /** Access to individual beacons */
107  const TGaussianMode& get(size_t i) const {
108  ASSERT_(i<m_modes.size())
109  return m_modes[i];
110  }
111  /** Access to individual beacons */
112  TGaussianMode& get(size_t i) {
113  ASSERT_(i<m_modes.size())
114  return m_modes[i];
115  }
116 
117  /** Inserts a copy of the given mode into the SOG */
118  void push_back(const TGaussianMode& m) {
119  m_modes.push_back(m);
120  }
121 
122  iterator begin() { return m_modes.begin(); }
123  iterator end() { return m_modes.end(); }
124  const_iterator begin() const { return m_modes.begin(); }
125  const_iterator end()const { return m_modes.end(); }
126 
127  iterator erase(iterator i) { return m_modes.erase(i); }
128 
129  void resize(const size_t N); //!< Resize the number of SOG modes
130  size_t size() const { return m_modes.size(); } //!< Return the number of Gaussian modes.
131  bool empty() const { return m_modes.empty(); } //!< Return whether there is any Gaussian mode.
132 
133  /** Returns an estimate of the point, (the mean, or mathematical expectation of the PDF).
134  * \sa getCovariance
135  */
136  void getMean(CPoint3D &mean_point) const;
137 
138  /** Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once.
139  * \sa getMean
140  */
141  void getCovarianceAndMean(CMatrixDouble33 &cov,CPoint3D &mean_point) const;
142 
143  /** Normalize the weights in m_modes such as the maximum log-weight is 0.
144  */
145  void normalizeWeights();
146 
147  /** Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG) */
148  void getMostLikelyMode( CPointPDFGaussian& outVal ) const;
149 
150  /** 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]).
151  */
152  double ESS() const;
153 
154  /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
155  */
156  void copyFrom(const CPointPDF &o);
157 
158  /** Save the density to a text file, with the following format:
159  * There is one row per Gaussian "mode", and each row contains 10 elements:
160  * - w (The weight)
161  * - x_mean (gaussian mean value)
162  * - y_mean (gaussian mean value)
163  * - x_mean (gaussian mean value)
164  * - C11 (Covariance elements)
165  * - C22 (Covariance elements)
166  * - C33 (Covariance elements)
167  * - C12 (Covariance elements)
168  * - C13 (Covariance elements)
169  * - C23 (Covariance elements)
170  *
171  */
172  void saveToTextFile(const std::string &file) const;
173 
174  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
175  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
176  */
177  void changeCoordinatesReference(const CPose3D &newReferenceBase );
178 
179  /** Draw a sample from the pdf.
180  */
181  void drawSingleSample(CPoint3D &outSample) const;
182 
183  /** 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!)
184  * \param p1 The first distribution to fuse
185  * \param p2 The second distribution to fuse
186  * \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.
187  */
188  void bayesianFusion( const CPointPDF &p1, const CPointPDF &p2, const double &minMahalanobisDistToDrop = 0);
189 
190 
191  /** Evaluates the PDF within a rectangular grid and saves the result in a matrix (each row contains values for a fixed y-coordinate value).
192  */
193  void evaluatePDFInArea(
194  float x_min,
195  float x_max,
196  float y_min,
197  float y_max,
198  float resolutionXY,
199  float z,
200  CMatrixD &outMatrix,
201  bool sumOverAllZs = false );
202 
203  /** Evaluates the PDF at a given point.
204  */
205  double evaluatePDF(
206  const CPoint3D &x,
207  bool sumOverAllZs ) const;
208 
209 
210  }; // End of class def.
211 
212 
213  } // End of namespace
214 } // End of namespace
215 
216 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013