Main MRPT website > C++ reference for MRPT 1.9.9
CMonteCarloLocalization2D.cpp
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 
10 #include "slam-precomp.h" // Precompiled headerss
11 
13 
14 #include <mrpt/system/CTicTac.h>
17 #include <mrpt/obs/CSensoryFrame.h>
18 
19 #include <mrpt/random.h>
20 
22 
23 using namespace mrpt;
24 using namespace mrpt::bayes;
25 using namespace mrpt::poses;
26 using namespace mrpt::math;
27 using namespace mrpt::maps;
28 using namespace mrpt::obs;
29 using namespace mrpt::slam;
30 using namespace mrpt::random;
31 using namespace std;
32 
34 
35 namespace mrpt
36 {
37 namespace slam
38 {
39 /** Fills out a "TPoseBin2D" variable, given a path hypotesis and (if not set to
40  * nullptr) a new pose appended at the end, using the KLD params in "options".
41  */
42 template <>
44  mrpt::slam::detail::TPoseBin2D& outBin, const TKLDParams& opts,
45  const CMonteCarloLocalization2D::CParticleDataContent* currentParticleValue,
46  const TPose3D* newPoseToBeInserted)
47 {
48  // 2D pose approx: Use the latest pose only:
49  if (newPoseToBeInserted)
50  {
51  outBin.x = round(newPoseToBeInserted->x / opts.KLD_binSize_XY);
52  outBin.y = round(newPoseToBeInserted->y / opts.KLD_binSize_XY);
53  outBin.phi = round(newPoseToBeInserted->yaw / opts.KLD_binSize_PHI);
54  }
55  else
56  {
57  ASSERT_(currentParticleValue);
58  outBin.x = round(currentParticleValue->x / opts.KLD_binSize_XY);
59  outBin.y = round(currentParticleValue->y / opts.KLD_binSize_XY);
60  outBin.phi = round(currentParticleValue->phi / opts.KLD_binSize_PHI);
61  }
62 }
63 }
64 }
65 
67 
68 /*---------------------------------------------------------------
69  ctor
70  ---------------------------------------------------------------*/
71 // Passing a "this" pointer at this moment is not a problem since it will be NOT
72 // access until the object is fully initialized
73 CMonteCarloLocalization2D::CMonteCarloLocalization2D(size_t M)
75 {
76  this->setLoggerName("CMonteCarloLocalization2D");
77 }
78 
81  const size_t i, bool& is_valid_pose) const
82 {
83  if (i >= m_particles.size())
84  THROW_EXCEPTION("Particle index out of bounds!");
85  is_valid_pose = true;
86  return TPose3D(m_particles[i].d);
87 }
88 
89 /*---------------------------------------------------------------
90 
91  prediction_and_update_pfStandardProposal
92 
93  ---------------------------------------------------------------*/
95  const mrpt::obs::CActionCollection* actions,
96  const mrpt::obs::CSensoryFrame* sf,
98 {
100 
101  if (sf)
102  { // A map MUST be supplied!
103  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
104  if (!options.metricMap)
105  ASSERT_(options.metricMaps.size() == m_particles.size());
106  }
107 
108  PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(
109  actions, sf, PF_options, options.KLD_params);
110 
111  MRPT_END
112 }
113 
114 /*---------------------------------------------------------------
115 
116  prediction_and_update_pfAuxiliaryPFStandard
117 
118  ---------------------------------------------------------------*/
120  const mrpt::obs::CActionCollection* actions,
121  const mrpt::obs::CSensoryFrame* sf,
123 {
124  MRPT_START
125 
126  if (sf)
127  { // A map MUST be supplied!
128  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
129  if (!options.metricMap)
130  ASSERT_(options.metricMaps.size() == m_particles.size());
131  }
132 
135  actions, sf, PF_options, options.KLD_params);
136 
137  MRPT_END
138 }
139 
140 /*---------------------------------------------------------------
141 
142  prediction_and_update_pfAuxiliaryPFOptimal
143 
144  ---------------------------------------------------------------*/
146  const mrpt::obs::CActionCollection* actions,
147  const mrpt::obs::CSensoryFrame* sf,
149 {
150  MRPT_START
151 
152  if (sf)
153  { // A map MUST be supplied!
154  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
155  if (!options.metricMap)
156  ASSERT_(options.metricMaps.size() == m_particles.size());
157  }
158 
159  PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin2D>(
160  actions, sf, PF_options, options.KLD_params);
161 
162  MRPT_END
163 }
164 
165 /*---------------------------------------------------------------
166  PF_SLAM_computeObservationLikelihoodForParticle
167  ---------------------------------------------------------------*/
168 double
170  const CParticleFilter::TParticleFilterOptions& PF_options,
171  const size_t particleIndexForMap, const CSensoryFrame& observation,
172  const CPose3D& x) const
173 {
174  MRPT_UNUSED_PARAM(PF_options);
175  ASSERT_(
176  options.metricMap || particleIndexForMap < options.metricMaps.size());
177 
178  CMetricMap* map =
179  (options.metricMap) ? options.metricMap : // All particles, one map
180  options.metricMaps[particleIndexForMap]; // One map per particle
181 
182  // For each observation:
183  double ret = 1;
184  for (CSensoryFrame::const_iterator it = observation.begin();
185  it != observation.end(); ++it)
186  ret += map->computeObservationLikelihood(
187  it->get(), x); // Compute the likelihood:
188 
189  // Done!
190  return ret;
191 }
192 
193 // Specialization for my kind of particles:
196  TPose2D* particleData, const TPose3D& newPose) const
197 {
198  *particleData = TPose2D(newPose);
199 }
200 
202  CParticleList& old_particles, const vector<TPose3D>& newParticles,
203  const vector<double>& newParticlesWeight,
204  const vector<size_t>& newParticlesDerivedFromIdx) const
205 {
206  MRPT_UNUSED_PARAM(newParticlesDerivedFromIdx);
208  size_t(newParticlesWeight.size()), size_t(newParticles.size()));
209 
210  // ---------------------------------------------------------------------------------
211  // Substitute old by new particle set:
212  // Old are in "m_particles"
213  // New are in "newParticles",
214  // "newParticlesWeight","newParticlesDerivedFromIdx"
215  // ---------------------------------------------------------------------------------
216  // Free old m_particles: not needed since "d" is now a smart ptr
217 
218  // Copy into "m_particles"
219  const size_t N = newParticles.size();
220  old_particles.resize(N);
221  for (size_t i = 0; i < N; i++)
222  {
223  old_particles[i].log_w = newParticlesWeight[i];
224  old_particles[i].d = TPose2D(newParticles[i]);
225  }
226 }
227 
229  COccupancyGridMap2D* theMap, const double freeCellsThreshold,
230  const int particlesCount, const double x_min, const double x_max,
231  const double y_min, const double y_max, const double phi_min,
232  const double phi_max)
233 {
234  MRPT_START
235 
236  ASSERT_(theMap != nullptr);
237  int sizeX = theMap->getSizeX();
238  int sizeY = theMap->getSizeY();
239  double gridRes = theMap->getResolution();
240  std::vector<double> freeCells_x, freeCells_y;
241  size_t nFreeCells;
242  unsigned int xIdx1, xIdx2;
243  unsigned int yIdx1, yIdx2;
244 
245  freeCells_x.reserve(sizeX * sizeY);
246  freeCells_y.reserve(sizeX * sizeY);
247 
248  if (x_min > theMap->getXMin())
249  xIdx1 = max(0, theMap->x2idx(x_min));
250  else
251  xIdx1 = 0;
252  if (x_max < theMap->getXMax())
253  xIdx2 = min(sizeX - 1, theMap->x2idx(x_max));
254  else
255  xIdx2 = sizeX - 1;
256  if (y_min > theMap->getYMin())
257  yIdx1 = max(0, theMap->y2idx(y_min));
258  else
259  yIdx1 = 0;
260  if (y_max < theMap->getYMax())
261  yIdx2 = min(sizeY - 1, theMap->y2idx(y_max));
262  else
263  yIdx2 = sizeY - 1;
264 
265  for (unsigned int x = xIdx1; x <= xIdx2; x++)
266  for (unsigned int y = yIdx1; y <= yIdx2; y++)
267  if (theMap->getCell(x, y) >= freeCellsThreshold)
268  {
269  freeCells_x.push_back(theMap->idx2x(x));
270  freeCells_y.push_back(theMap->idx2y(y));
271  }
272 
273  nFreeCells = freeCells_x.size();
274 
275  // Assure that map is not fully occupied!
276  ASSERT_(nFreeCells);
277 
278  if (particlesCount > 0)
279  m_particles.resize(particlesCount);
280 
281  const size_t M = m_particles.size();
282  // Generate pose m_particles:
283  for (size_t i = 0; i < M; i++)
284  {
285  int idx =
286  round(getRandomGenerator().drawUniform(0.0, nFreeCells - 1.001));
287 
288  m_particles[i].d.x=
289  freeCells_x[idx] +
290  getRandomGenerator().drawUniform(-gridRes, gridRes);
291  m_particles[i].d.y=
292  freeCells_y[idx] +
293  getRandomGenerator().drawUniform(-gridRes, gridRes);
294  m_particles[i].d.phi =
295  getRandomGenerator().drawUniform(phi_min, phi_max);
296  m_particles[i].log_w = 0;
297  }
298 
299  MRPT_END
300 }
COccupancyGridMap2D.h
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
mrpt::slam::PF_implementation< mrpt::math::TPose2D, CMonteCarloLocalization2D, mrpt::poses::CPosePDFParticles::PARTICLE_STORAGE >::PF_SLAM_implementation_pfAuxiliaryPFStandard
void PF_SLAM_implementation_pfAuxiliaryPFStandard(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options)
A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary pa...
Definition: PF_implementations.h:403
mrpt::obs::CSensoryFrame::end
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage:
Definition: CSensoryFrame.h:260
mrpt::slam::KLF_loadBinFromParticle
void KLF_loadBinFromParticle(mrpt::slam::detail::TPoseBin2D &outBin, const TKLDParams &opts, const CMonteCarloLocalization2D::CParticleDataContent *currentParticleValue, const TPose3D *newPoseToBeInserted)
Fills out a "TPoseBin2D" variable, given a path hypotesis and (if not set to nullptr) a new pose appe...
Definition: CMonteCarloLocalization2D.cpp:43
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: lightweight_geom_data.h:195
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
CMonteCarloLocalization2D.h
mrpt::slam::TMonteCarloLocalizationParams::metricMaps
mrpt::maps::TMetricMapList metricMaps
[update stage] Alternative way (if metricMap==nullptr): A metric map is supplied for each particle: T...
Definition: TMonteCarloLocalizationParams.h:47
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:20
mrpt::maps::CMetricMap::computeObservationLikelihood
double computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom)
Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.
Definition: CMetricMap.cpp:189
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::math::TPose2D::y
double y
Definition: lightweight_geom_data.h:193
mrpt::slam::CMonteCarloLocalization2D::~CMonteCarloLocalization2D
virtual ~CMonteCarloLocalization2D()
Destructor.
Definition: CMonteCarloLocalization2D.cpp:79
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
PF_implementations.h
mrpt::math::TPose3D::y
double y
Definition: lightweight_geom_data.h:610
mrpt::slam::detail::TPoseBin2D::y
int y
Definition: PF_aux_structs.h:32
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
CActionCollection.h
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
random.h
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::maps::COccupancyGridMap2D::idx2y
float idx2y(const size_t cy) const
Definition: COccupancyGridMap2D.h:333
mrpt::maps::COccupancyGridMap2D::getCell
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
Definition: COccupancyGridMap2D.h:379
mrpt::maps::COccupancyGridMap2D::getSizeY
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: COccupancyGridMap2D.h:298
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
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::slam::detail::TPoseBin2D::phi
int phi
Definition: PF_aux_structs.h:32
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
mrpt::random::CRandomGenerator::drawUniform
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
Definition: RandomGenerators.h:111
mrpt::obs::CSensoryFrame::begin
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage:
Definition: CSensoryFrame.h:247
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23
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_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::maps::COccupancyGridMap2D::y2idx
int y2idx(float y) const
Definition: COccupancyGridMap2D.h:314
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::maps::COccupancyGridMap2D::x2idx
int x2idx(float x) const
Transform a coordinate value into a cell index.
Definition: COccupancyGridMap2D.h:310
mrpt::slam::TKLDParams::KLD_binSize_PHI
double KLD_binSize_PHI
Definition: TKLDParams.h:34
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:186
mrpt::math::TPose3D::x
double x
X,Y,Z, coords.
Definition: lightweight_geom_data.h:610
mrpt::maps::COccupancyGridMap2D::getYMin
float getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: COccupancyGridMap2D.h:304
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
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:603
mrpt::slam::detail::TPoseBin2D
Auxiliary structure used in KLD-sampling in particle filters.
Definition: PF_aux_structs.h:28
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::obs::CSensoryFrame::const_iterator
std::deque< CObservation::Ptr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
Definition: CSensoryFrame.h:233
mrpt::slam::detail::TPoseBin2D::x
int x
Bin indices.
Definition: PF_aux_structs.h:32
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
PF_aux_structs.h
CSensoryFrame.h
mrpt::bayes::CParticleFilterData< mrpt::math::TPose2D, mrpt::bayes::particle_storage_mode::VALUE >::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:218
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::maps::COccupancyGridMap2D::getResolution
float getResolution() const
Returns the resolution of the grid map.
Definition: COccupancyGridMap2D.h:308
mrpt::math::TPose3D::yaw
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: lightweight_geom_data.h:612
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:19
CTicTac.h
mrpt::slam::CMonteCarloLocalization2D::options
TMonteCarloLocalizationParams options
MCL parameters.
Definition: CMonteCarloLocalization2D.h:46
mrpt::maps::COccupancyGridMap2D::getXMin
float getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: COccupancyGridMap2D.h:300
slam-precomp.h
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:32
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::maps::COccupancyGridMap2D::idx2x
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
Definition: COccupancyGridMap2D.h:329
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:62
mrpt::maps::COccupancyGridMap2D::getSizeX
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: COccupancyGridMap2D.h:296
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:17
mrpt::slam::TMonteCarloLocalizationParams::metricMap
mrpt::maps::CMetricMap * metricMap
[update stage] Must be set to a metric map used to estimate the likelihood of observations
Definition: TMonteCarloLocalizationParams.h:41
mrpt::maps
Definition: CBeacon.h:24
mrpt::system::COutputLogger::setLoggerName
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
Definition: COutputLogger.cpp:127
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
mrpt::slam
Definition: CMultiMetricMapPDF.h:27
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:102
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::slam::TKLDParams::KLD_binSize_XY
double KLD_binSize_XY
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...
Definition: TKLDParams.h:34
mrpt::slam::TMonteCarloLocalizationParams::KLD_params
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
Definition: TMonteCarloLocalizationParams.h:50
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: lightweight_geom_data.h:193
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