Main MRPT website > C++ reference
MRPT logo
CPosePDFParticles.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 CPosePDFParticles_H
29 #define CPosePDFParticles_H
30 
31 #include <mrpt/poses/CPosePDF.h>
32 #include <mrpt/poses/CPoseRandomSampler.h>
33 #include <mrpt/bayes/CParticleFilterCapable.h>
34 #include <mrpt/bayes/CParticleFilterData.h>
35 
36 namespace mrpt
37 {
38  namespace poses
39  {
40  using namespace mrpt::bayes;
41 
42  // This must be added to any CSerializable derived class:
43  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFParticles , CPosePDF )
44 
45  /** Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.
46  *
47  * This class is also the base for the implementation of Monte-Carlo Localization (MCL), in mrpt::slam::CMonteCarloLocalization2D.
48  *
49  * See the application "app/pf-localization" for an example of usage.
50  *
51  * \sa CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable
52  * \ingroup poses_pdf_grp
53  */
54  class BASE_IMPEXP CPosePDFParticles :
55  public CPosePDF,
56  public mrpt::bayes::CParticleFilterData<CPose2D>,
57  public mrpt::bayes::CParticleFilterCapable
58  {
59  // This must be added to any CSerializable derived class:
60  DEFINE_SERIALIZABLE( CPosePDFParticles )
61 
62  // This uses CParticleFilterData to implement some methods required for CParticleFilterCapable:
63  IMPLEMENT_PARTICLE_FILTER_CAPABLE(CPose2D);
64 
65  public:
66  /** Free all the memory associated to m_particles, and set the number of parts = 0
67  */
68  void clear();
69 
70  /** Constructor
71  * \param M The number of m_particles.
72  */
73  CPosePDFParticles( size_t M = 1 );
74 
75  /** Copy constructor:
76  */
77  inline CPosePDFParticles( const CPosePDFParticles& obj )
78  {
79  copyFrom( obj );
80  }
81 
82  /** Destructor
83  */
84  virtual ~CPosePDFParticles();
85 
86  /** Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
87  */
88  void copyFrom(const CPosePDF &o);
89 
90  /** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
91  * \param location The location to set all the m_particles.
92  * \param particlesCount If this is set to 0 the number of m_particles remains unchanged.
93  * \sa resetUniform, resetUniformFreeSpace
94  */
95  void resetDeterministic(
96  const CPose2D &location,
97  size_t particlesCount = 0);
98 
99  /** Reset the PDF to an uniformly distributed one, inside of the defined cube.
100  * If particlesCount is set to -1 the number of m_particles remains unchanged.
101  * \sa resetDeterministic, resetUniformFreeSpace
102  */
103  void resetUniform(
104  const double & x_min,
105  const double & x_max,
106  const double & y_min,
107  const double & y_max,
108  const double & phi_min = -M_PI,
109  const double & phi_max = M_PI,
110  const int &particlesCount = -1);
111 
112  /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
113  * \sa getCovariance
114  */
115  void getMean(CPose2D &mean_pose) const;
116 
117  /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
118  * \sa getMean
119  */
120  void getCovarianceAndMean(CMatrixDouble33 &cov,CPose2D &mean_point) const;
121 
122  /** Returns the pose of the i'th particle.
123  */
124  CPose2D getParticlePose(size_t i) const;
125 
126  /** Save PDF's m_particles to a text file. In each line it will go: "x y phi weight"
127  */
128  void saveToTextFile(const std::string &file) const;
129 
130  /** Get the m_particles count (equivalent to "particlesCount")
131  */
132  inline size_t size() const { return m_particles.size(); }
133 
134  /** Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly.
135  */
136  void performSubstitution( std::vector<int> &indx );
137 
138  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
139  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
140  */
141  void changeCoordinatesReference( const CPose3D &newReferenceBase );
142 
143  /** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
144  */
145  void drawSingleSample(CPose2D &outPart ) const;
146 
147  /** Appends (pose-composition) a given pose "p" to each particle
148  */
149  void operator += ( const CPose2D &Ap);
150 
151  /** Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
152  */
153  void append( CPosePDFParticles &o );
154 
155  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
156  */
157  void inverse(CPosePDF &o) const;
158 
159  /** Returns the particle with the highest weight.
160  */
161  CPose2D getMostLikelyParticle() const;
162 
163  /** Bayesian fusion.
164  */
165  void bayesianFusion( const CPosePDF &p1,const CPosePDF &p2, const double & minMahalanobisDistToDrop = 0 );
166 
167  /** Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window.
168  * \sa saveParzenPDFToTextFile
169  */
170  double evaluatePDF_parzen(
171  const double & x,
172  const double & y,
173  const double & phi,
174  const double & stdXY,
175  const double & stdPhi ) const;
176 
177  /** Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window.
178  * \sa evaluatePDF_parzen
179  */
180  void saveParzenPDFToTextFile(
181  const char *fileName,
182  const double & x_min,
183  const double & x_max,
184  const double & y_min,
185  const double & y_max,
186  const double & phi,
187  const double & stepSizeXY,
188  const double & stdXY,
189  const double & stdPhi ) const;
190 
191  }; // End of class def.
192 
193  } // End of namespace
194 } // End of namespace
195 
196 #endif


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