Main MRPT website > C++ reference for MRPT 1.4.0
CMonteCarloLocalization2D.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 CMonteCarloLocalization2D_H
10 #define CMonteCarloLocalization2D_H
11 
15 #include <mrpt/obs/obs_frwds.h>
16 
17 #include <mrpt/slam/link_pragmas.h>
18 
19 namespace mrpt
20 {
21  namespace maps { class COccupancyGridMap2D; }
22 
23  /** \ingroup mrpt_slam_grp */
24  namespace slam
25  {
26  /** Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.
27  *
28  * This class also implements particle filtering for robot localization. See the MRPT
29  * application "app/pf-localization" for an example of usage.
30  *
31  * \sa CMonteCarloLocalization3D, CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable
32  * \ingroup mrpt_slam_grp
33  */
36  public PF_implementation<mrpt::poses::CPose2D,CMonteCarloLocalization2D>
37  {
38  public:
40 
41  /** Constructor
42  * \param M The number of m_particles.
43  */
44  CMonteCarloLocalization2D( size_t M = 1 );
45 
46  /** Destructor */
47  virtual ~CMonteCarloLocalization2D();
48 
49  /** Reset the PDF to an uniformly distributed one, but only in the free-space
50  * of a given 2D occupancy-grid-map. Orientation is randomly generated in the whole 2*PI range.
51  * \param theMap The occupancy grid map
52  * \param freeCellsThreshold The minimum free-probability to consider a cell as empty (default is 0.7)
53  * \param particlesCount If set to -1 the number of m_particles remains unchanged.
54  * \param x_min The limits of the area to look for free cells.
55  * \param x_max The limits of the area to look for free cells.
56  * \param y_min The limits of the area to look for free cells.
57  * \param y_max The limits of the area to look for free cells.
58  * \param phi_min The limits of the area to look for free cells.
59  * \param phi_max The limits of the area to look for free cells.
60  * \sa resetDeterm32inistic
61  * \exception std::exception On any error (no free cell found in map, map=NULL, etc...)
62  */
63  void resetUniformFreeSpace(
65  const double freeCellsThreshold = 0.7,
66  const int particlesCount = -1,
67  const double x_min = -1e10f,
68  const double x_max = 1e10f,
69  const double y_min = -1e10f,
70  const double y_max = 1e10f,
71  const double phi_min = -M_PI,
72  const double phi_max = M_PI );
73 
74  /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
75  * This method has additional configuration parameters in "options".
76  * Performs the update stage of the RBPF, using the sensed CSensoryFrame:
77  *
78  * \param action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
79  * \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
80  *
81  * \sa options
82  */
83  void prediction_and_update_pfStandardProposal(
84  const mrpt::obs::CActionCollection * action,
85  const mrpt::obs::CSensoryFrame * observation,
87 
88  /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
89  * This method has additional configuration parameters in "options".
90  * Performs the update stage of the RBPF, using the sensed CSensoryFrame:
91  *
92  * \param Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
93  * \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
94  *
95  * \sa options
96  */
97  void prediction_and_update_pfAuxiliaryPFStandard(
98  const mrpt::obs::CActionCollection * action,
99  const mrpt::obs::CSensoryFrame * observation,
101 
102  /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
103  * This method has additional configuration parameters in "options".
104  * Performs the update stage of the RBPF, using the sensed CSensoryFrame:
105  *
106  * \param Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
107  * \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
108  *
109  * \sa options
110  */
111  void prediction_and_update_pfAuxiliaryPFOptimal(
112  const mrpt::obs::CActionCollection * action,
113  const mrpt::obs::CSensoryFrame * observation,
115 
116  //protected:
117  /** \name Virtual methods that the PF_implementations assume exist.
118  @{ */
119  /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
120  const mrpt::math::TPose3D * getLastPose(const size_t i) const;
121 
122  void PF_SLAM_implementation_custom_update_particle_with_new_pose(
123  CParticleDataContent *particleData,
124  const mrpt::math::TPose3D &newPose) const;
125 
126  // We'll redefine this one:
127  void PF_SLAM_implementation_replaceByNewParticleSet(
128  CParticleList &old_particles,
129  const std::vector<mrpt::math::TPose3D> &newParticles,
130  const std::vector<double> &newParticlesWeight,
131  const std::vector<size_t> &newParticlesDerivedFromIdx ) const;
132 
133  /** Evaluate the observation likelihood for one particle at a given location */
134  double PF_SLAM_computeObservationLikelihoodForParticle(
136  const size_t particleIndexForMap,
137  const mrpt::obs::CSensoryFrame &observation,
138  const mrpt::poses::CPose3D &x ) const;
139  /** @} */
140 
141 
142  }; // End of class def.
143 
144  } // End of namespace
145 } // End of namespace
146 
147 #endif
mrpt::bayes::CParticleFilterData< CPose2D >::CParticleList
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
Definition: CParticleFilterData.h:182
mrpt::slam::CMonteCarloLocalization2D
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,...
Definition: CMonteCarloLocalization2D.h:34
TMonteCarloLocalizationParams.h
PF_implementations_data.h
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: obs/CActionCollection.h:30
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: obs/CSensoryFrame.h:52
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::slam::PF_implementation
A set of common data shared by PF implementations for both SLAM and localization.
Definition: PF_implementations_data.h:40
obs_frwds.h
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:389
mrpt::poses::CPosePDFParticles
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,...
Definition: CPosePDFParticles.h:33
mrpt::slam::CMonteCarloLocalization2D::options
TMonteCarloLocalizationParams options
MCL parameters.
Definition: CMonteCarloLocalization2D.h:39
mrpt::slam::TMonteCarloLocalizationParams
The struct for passing extra simulation parameters to the prediction stage when running a particle fi...
Definition: TMonteCarloLocalizationParams.h:25
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: maps/COccupancyGridMap2D.h:61
M_PI
#define M_PI
Definition: bits.h:65
CPosePDFParticles.h
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:80
mrpt::bayes::CParticleFilterData< CPose2D >::CParticleDataContent
CPose2D CParticleDataContent
This is the type inside the corresponding CParticleData class.
Definition: CParticleFilterData.h:180



Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 23:08:25 UTC 2019