Main MRPT website > C++ reference
MRPT logo
CMonteCarloLocalization2D.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 CMonteCarloLocalization2D_H
29 #define CMonteCarloLocalization2D_H
30 
31 #include <mrpt/poses/CPosePDFParticles.h>
32 #include <mrpt/slam/PF_implementations_data.h>
33 #include <mrpt/slam/TMonteCarloLocalizationParams.h>
34 
35 #include <mrpt/slam/link_pragmas.h>
36 
37 namespace mrpt
38 {
39  /** \ingroup mrpt_slam_grp */
40  namespace slam
41  {
42  class COccupancyGridMap2D;
43  class CSensoryFrame;
44 
45  using namespace mrpt::poses;
46  using namespace mrpt::slam;
47  using namespace mrpt::bayes;
48 
49  /** Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.
50  *
51  * This class also implements particle filtering for robot localization. See the MRPT
52  * application "app/pf-localization" for an example of usage.
53  *
54  * \sa CMonteCarloLocalization3D, CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable
55  * \ingroup mrpt_slam_grp
56  */
57  class SLAM_IMPEXP CMonteCarloLocalization2D :
58  public CPosePDFParticles,
59  public PF_implementation<CPose2D,CMonteCarloLocalization2D>
60  {
61  public:
62  TMonteCarloLocalizationParams options; //!< MCL parameters
63 
64  /** Constructor
65  * \param M The number of m_particles.
66  */
67  CMonteCarloLocalization2D( size_t M = 1 );
68 
69  /** Destructor */
70  virtual ~CMonteCarloLocalization2D();
71 
72  /** Reset the PDF to an uniformly distributed one, but only in the free-space
73  * of a given 2D occupancy-grid-map. Orientation is randomly generated in the whole 2*PI range.
74  * \param theMap The occupancy grid map
75  * \param freeCellsThreshold The minimum free-probability to consider a cell as empty (default is 0.7)
76  * \param particlesCount If set to -1 the number of m_particles remains unchanged.
77  * \param x_min The limits of the area to look for free cells.
78  * \param x_max The limits of the area to look for free cells.
79  * \param y_min The limits of the area to look for free cells.
80  * \param y_max The limits of the area to look for free cells.
81  * \param phi_min The limits of the area to look for free cells.
82  * \param phi_max The limits of the area to look for free cells.
83  * \sa resetDeterm32inistic
84  * \exception std::exception On any error (no free cell found in map, map=NULL, etc...)
85  */
86  void resetUniformFreeSpace(
87  COccupancyGridMap2D *theMap,
88  const double freeCellsThreshold = 0.7,
89  const int particlesCount = -1,
90  const double x_min = -1e10f,
91  const double x_max = 1e10f,
92  const double y_min = -1e10f,
93  const double y_max = 1e10f,
94  const double phi_min = -M_PI,
95  const double phi_max = M_PI );
96 
97  /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
98  * This method has additional configuration parameters in "options".
99  * Performs the update stage of the RBPF, using the sensed CSensoryFrame:
100  *
101  * \param action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
102  * \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
103  *
104  * \sa options
105  */
106  void prediction_and_update_pfStandardProposal(
107  const mrpt::slam::CActionCollection * action,
108  const mrpt::slam::CSensoryFrame * observation,
109  const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
110 
111  /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
112  * This method has additional configuration parameters in "options".
113  * Performs the update stage of the RBPF, using the sensed CSensoryFrame:
114  *
115  * \param Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
116  * \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
117  *
118  * \sa options
119  */
120  void prediction_and_update_pfAuxiliaryPFStandard(
121  const mrpt::slam::CActionCollection * action,
122  const mrpt::slam::CSensoryFrame * observation,
123  const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
124 
125  /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
126  * This method has additional configuration parameters in "options".
127  * Performs the update stage of the RBPF, using the sensed CSensoryFrame:
128  *
129  * \param Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
130  * \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
131  *
132  * \sa options
133  */
134  void prediction_and_update_pfAuxiliaryPFOptimal(
135  const mrpt::slam::CActionCollection * action,
136  const mrpt::slam::CSensoryFrame * observation,
137  const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
138 
139  //protected:
140  /** \name Virtual methods that the PF_implementations assume exist.
141  @{ */
142  /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
143  const TPose3D * getLastPose(const size_t i) const;
144 
145  void PF_SLAM_implementation_custom_update_particle_with_new_pose(
146  CParticleDataContent *particleData,
147  const TPose3D &newPose) const;
148 
149  // We'll redefine this one:
150  void PF_SLAM_implementation_replaceByNewParticleSet(
151  CParticleList &old_particles,
152  const std::vector<TPose3D> &newParticles,
153  const vector_double &newParticlesWeight,
154  const std::vector<size_t> &newParticlesDerivedFromIdx ) const;
155 
156  /** Evaluate the observation likelihood for one particle at a given location */
157  double PF_SLAM_computeObservationLikelihoodForParticle(
158  const CParticleFilter::TParticleFilterOptions &PF_options,
159  const size_t particleIndexForMap,
160  const CSensoryFrame &observation,
161  const CPose3D &x ) const;
162  /** @} */
163 
164 
165  }; // End of class def.
166 
167  } // End of namespace
168 } // End of namespace
169 
170 #endif


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