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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020