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



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST