Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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/utils/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 mrpt::utils;
32 using namespace std;
33 
35 
36 namespace mrpt
37 {
38  namespace slam
39  {
40  /** Fills out a "TPoseBin2D" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options". */
41  template <>
44  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 access until the object is fully initialized
72 CMonteCarloLocalization2D::CMonteCarloLocalization2D( size_t M ) :
74 {
75  this->setLoggerName("CMonteCarloLocalization2D");
76 }
77 
78 
79 /*---------------------------------------------------------------
80  Dtor
81  ---------------------------------------------------------------*/
83 {
84 }
85 
86 /*---------------------------------------------------------------
87  getLastPose
88  ---------------------------------------------------------------*/
89 TPose3D CMonteCarloLocalization2D::getLastPose(const size_t i, bool &is_valid_pose) const
90 {
91  if (i>=m_particles.size()) THROW_EXCEPTION("Particle index out of bounds!");
92  is_valid_pose = true;
93  ASSERTDEB_(m_particles[i].d!=NULL)
94  return TPose3D( TPose2D(*m_particles[i].d));
95 }
96 
97 /*---------------------------------------------------------------
98 
99  prediction_and_update_pfStandardProposal
100 
101  ---------------------------------------------------------------*/
103  const mrpt::obs::CActionCollection * actions,
104  const mrpt::obs::CSensoryFrame * sf,
106 {
107  MRPT_START
108 
109  if (sf)
110  { // A map MUST be supplied!
112  if (!options.metricMap)
113  ASSERT_(options.metricMaps.size() == m_particles.size() )
114  }
115 
116  PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(actions, sf, PF_options,options.KLD_params);
117 
118  MRPT_END
119 }
120 
121 /*---------------------------------------------------------------
122 
123  prediction_and_update_pfAuxiliaryPFStandard
124 
125  ---------------------------------------------------------------*/
127  const mrpt::obs::CActionCollection * actions,
128  const mrpt::obs::CSensoryFrame * sf,
130 {
131  MRPT_START
132 
133  if (sf)
134  { // A map MUST be supplied!
136  if (!options.metricMap)
137  ASSERT_(options.metricMaps.size() == m_particles.size() )
138  }
139 
140  PF_SLAM_implementation_pfAuxiliaryPFStandard<mrpt::slam::detail::TPoseBin2D>(actions, sf, PF_options,options.KLD_params);
141 
142  MRPT_END
143 }
144 
145 
146 /*---------------------------------------------------------------
147 
148  prediction_and_update_pfAuxiliaryPFOptimal
149 
150  ---------------------------------------------------------------*/
152  const mrpt::obs::CActionCollection * actions,
153  const mrpt::obs::CSensoryFrame * sf,
155 {
156  MRPT_START
157 
158  if (sf)
159  { // A map MUST be supplied!
161  if (!options.metricMap)
162  ASSERT_(options.metricMaps.size() == m_particles.size() )
163  }
164 
165  PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin2D>(actions, sf, PF_options,options.KLD_params);
166 
167  MRPT_END
168 }
169 
170 
171 /*---------------------------------------------------------------
172  PF_SLAM_computeObservationLikelihoodForParticle
173  ---------------------------------------------------------------*/
175  const CParticleFilter::TParticleFilterOptions &PF_options,
176  const size_t particleIndexForMap,
177  const CSensoryFrame &observation,
178  const CPose3D &x ) const
179 {
180  MRPT_UNUSED_PARAM(PF_options);
181  ASSERT_( options.metricMap || particleIndexForMap<options.metricMaps.size() )
182 
183  CMetricMap *map = (options.metricMap) ?
184  options.metricMap : // All particles, one map
185  options.metricMaps[particleIndexForMap]; // One map per particle
186 
187  // For each observation:
188  double ret = 1;
189  for (CSensoryFrame::const_iterator it=observation.begin();it!=observation.end();++it)
190  ret += map->computeObservationLikelihood( it->pointer(), x ); // Compute the likelihood:
191 
192  // Done!
193  return ret;
194 }
195 
196 // Specialization for my kind of particles:
198  CPose2D *particleData,
199  const TPose3D &newPose) const
200 {
201  *particleData = CPose2D( TPose2D(newPose) );
202 }
203 
204 
206  CParticleList &old_particles,
207  const vector<TPose3D> &newParticles,
208  const vector<double> &newParticlesWeight,
209  const vector<size_t> &newParticlesDerivedFromIdx ) const
210 {
211  MRPT_UNUSED_PARAM(newParticlesDerivedFromIdx);
212  ASSERT_EQUAL_(size_t(newParticlesWeight.size()), size_t(newParticles.size()));
213 
214  // ---------------------------------------------------------------------------------
215  // Substitute old by new particle set:
216  // Old are in "m_particles"
217  // New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
218  // ---------------------------------------------------------------------------------
219  // Free old m_particles: not needed since "d" is now a smart ptr
220 
221  // Copy into "m_particles"
222  const size_t N = newParticles.size();
223  old_particles.resize(N);
224  for (size_t i=0;i<N;i++)
225  {
226  old_particles[i].log_w = newParticlesWeight[i];
227  old_particles[i].d.reset(new CPose2D(TPose2D(newParticles[i])));
228  }
229 }
230 
232  COccupancyGridMap2D *theMap,
233  const double freeCellsThreshold ,
234  const int particlesCount ,
235  const double x_min ,
236  const double x_max ,
237  const double y_min ,
238  const double y_max ,
239  const double phi_min,
240  const double phi_max)
241 {
242  MRPT_START
243 
244  ASSERT_(theMap!=NULL)
245 
246  int sizeX = theMap->getSizeX();
247  int sizeY = theMap->getSizeY();
248  double gridRes = theMap->getResolution();
249  std::vector<double> freeCells_x,freeCells_y;
250  size_t nFreeCells;
251  unsigned int xIdx1,xIdx2;
252  unsigned int yIdx1,yIdx2;
253 
254  freeCells_x.reserve( sizeX * sizeY );
255  freeCells_y.reserve( sizeX * sizeY );
256 
257  if (x_min>theMap->getXMin())
258  xIdx1 = max(0, theMap->x2idx( x_min ) );
259  else xIdx1 = 0;
260  if (x_max<theMap->getXMax())
261  xIdx2 = min(sizeX-1, theMap->x2idx( x_max ) );
262  else xIdx2 = sizeX-1;
263  if (y_min>theMap->getYMin())
264  yIdx1 = max(0, theMap->y2idx( y_min ) );
265  else yIdx1 = 0;
266  if (y_max<theMap->getYMax())
267  yIdx2 = min(sizeY-1, theMap->y2idx( y_max ) );
268  else yIdx2 = sizeY-1;
269 
270 
271  for (unsigned int x=xIdx1;x<=xIdx2;x++)
272  for (unsigned int y=yIdx1;y<=yIdx2;y++)
273  if (theMap->getCell(x,y)>=freeCellsThreshold)
274  {
275  freeCells_x.push_back(theMap->idx2x(x));
276  freeCells_y.push_back(theMap->idx2y(y));
277  }
278 
279  nFreeCells = freeCells_x.size();
280 
281  // Assure that map is not fully occupied!
282  ASSERT_( nFreeCells );
283 
284 
285  if (particlesCount>0)
286  {
287  clear();
288  m_particles.resize(particlesCount);
289  for (int i = 0; i < particlesCount; i++)
290  m_particles[i].d.reset(new CPose2D());
291  }
292 
293  const size_t M = m_particles.size();
294 
295  // Generate pose m_particles:
296  for (size_t i=0;i<M;i++)
297  {
298  int idx = round(randomGenerator.drawUniform(0.0,nFreeCells-1.001));
299 
300  m_particles[i].d->x( freeCells_x[idx] + randomGenerator.drawUniform( -gridRes, gridRes ) );
301  m_particles[i].d->y( freeCells_y[idx] + randomGenerator.drawUniform( -gridRes, gridRes ) );
302  m_particles[i].d->phi( randomGenerator.drawUniform( phi_min, phi_max ) );
303  m_particles[i].log_w=0;
304  }
305 
306  MRPT_END
307 }
#define ASSERT_EQUAL_(__A, __B)
A namespace of pseudo-random numbers genrators of diferent distributions.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
float getYMin() const
Returns the "y" coordinate of top side of grid map.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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
#define min(a, b)
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...
float getResolution() const
Returns the resolution of the grid map.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
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
Evaluate the observation likelihood for one particle at a given location.
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
Option set for KLD algorithm.
Definition: TKLDParams.h:22
TMonteCarloLocalizationParams options
MCL parameters.
STL namespace.
mrpt::math::TPose3D getLastPose(const size_t i, bool &pose_is_valid) const MRPT_OVERRIDE
Return the last robot pose in the i&#39;th particle; pose_is_valid will be false if particle is a path an...
void clear()
Free all the memory associated to m_particles, and set the number of parts = 0.
mrpt::maps::TMetricMapList metricMaps
[update stage] Alternative way (if metricMap==NULL): A metric map is supplied for each particle: Ther...
double yaw
Yaw coordinate (rotation angle over Z axis).
void PF_SLAM_implementation_custom_update_particle_with_new_pose(CParticleDataContent *particleData, const mrpt::math::TPose3D &newPose) const
mrpt::maps::CMetricMap * metricMap
[update stage] Must be set to a metric map used to estimate the likelihood of observations ...
Declares a class for storing a collection of robot actions.
std::deque< CObservationPtr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
CParticleList m_particles
The array of particles.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_END
float getXMin() const
Returns the "x" coordinate of left side of grid map.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
double KLD_binSize_XY
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox&#39;s papers), which is used only i...
Definition: TKLDParams.h:32
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
Auxiliary structure used in KLD-sampling in particle filters.
A class for storing an occupancy grid map.
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
The configuration of a particle filter.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Lightweight 2D pose.
void prediction_and_update_pfAuxiliaryPFStandard(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Update the m_particles, predicting the posterior of robot pose and map after a movement command...
#define ASSERT_(f)
float idx2y(const size_t cy) const
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
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 NULL) a new pose appende...
GLenum GLint GLint y
Definition: glext.h:3516
void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Update the m_particles, predicting the posterior of robot pose and map after a movement command...
GLenum GLint x
Definition: glext.h:3516
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
int x2idx(float x) const
Transform a coordinate value into a cell index.
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
void prediction_and_update_pfStandardProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019