MRPT  1.9.9
CMonteCarloLocalization2D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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 
10 #include "slam-precomp.h" // Precompiled headerss
11 
13 
16 #include <mrpt/obs/CSensoryFrame.h>
17 #include <mrpt/system/CTicTac.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::slam
36 {
37 /** Fills out a "TPoseBin2D" variable, given a path hypotesis and (if not set to
38  * nullptr) a new pose appended at the end, using the KLD params in "options".
39  */
40 template <>
42  mrpt::slam::detail::TPoseBin2D& outBin, const TKLDParams& opts,
43  const CMonteCarloLocalization2D::CParticleDataContent* currentParticleValue,
44  const TPose3D* newPoseToBeInserted)
45 {
46  // 2D pose approx: Use the latest pose only:
47  if (newPoseToBeInserted)
48  {
49  outBin.x = round(newPoseToBeInserted->x / opts.KLD_binSize_XY);
50  outBin.y = round(newPoseToBeInserted->y / opts.KLD_binSize_XY);
51  outBin.phi = round(newPoseToBeInserted->yaw / opts.KLD_binSize_PHI);
52  }
53  else
54  {
55  ASSERT_(currentParticleValue);
56  outBin.x = round(currentParticleValue->x / opts.KLD_binSize_XY);
57  outBin.y = round(currentParticleValue->y / opts.KLD_binSize_XY);
58  outBin.phi = round(currentParticleValue->phi / opts.KLD_binSize_PHI);
59  }
60 }
61 } // namespace mrpt::slam
62 
64 
65 /*---------------------------------------------------------------
66  ctor
67  ---------------------------------------------------------------*/
68 // Passing a "this" pointer at this moment is not a problem since it will be NOT
69 // access until the object is fully initialized
70 CMonteCarloLocalization2D::CMonteCarloLocalization2D(size_t M)
72 {
73  this->setLoggerName("CMonteCarloLocalization2D");
74 }
75 
78  const size_t i, bool& is_valid_pose) const
79 {
80  if (i >= m_particles.size())
81  THROW_EXCEPTION("Particle index out of bounds!");
82  is_valid_pose = true;
83  return TPose3D(m_particles[i].d);
84 }
85 
86 /*---------------------------------------------------------------
87 
88  prediction_and_update_pfStandardProposal
89 
90  ---------------------------------------------------------------*/
92  const mrpt::obs::CActionCollection* actions,
93  const mrpt::obs::CSensoryFrame* sf,
95 {
97 
98  if (sf)
99  { // A map MUST be supplied!
100  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
101  if (!options.metricMap)
102  ASSERT_(options.metricMaps.size() == m_particles.size());
103  }
104 
105  PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(
106  actions, sf, PF_options, options.KLD_params);
107 
108  MRPT_END
109 }
110 
111 /*---------------------------------------------------------------
112 
113  prediction_and_update_pfAuxiliaryPFStandard
114 
115  ---------------------------------------------------------------*/
117  const mrpt::obs::CActionCollection* actions,
118  const mrpt::obs::CSensoryFrame* sf,
120 {
121  MRPT_START
122 
123  if (sf)
124  { // A map MUST be supplied!
125  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
126  if (!options.metricMap)
127  ASSERT_(options.metricMaps.size() == m_particles.size());
128  }
129 
132  actions, sf, PF_options, options.KLD_params);
133 
134  MRPT_END
135 }
136 
137 /*---------------------------------------------------------------
138 
139  prediction_and_update_pfAuxiliaryPFOptimal
140 
141  ---------------------------------------------------------------*/
143  const mrpt::obs::CActionCollection* actions,
144  const mrpt::obs::CSensoryFrame* sf,
146 {
147  MRPT_START
148 
149  if (sf)
150  { // A map MUST be supplied!
151  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
152  if (!options.metricMap)
153  ASSERT_(options.metricMaps.size() == m_particles.size());
154  }
155 
156  PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin2D>(
157  actions, sf, PF_options, options.KLD_params);
158 
159  MRPT_END
160 }
161 
162 /*---------------------------------------------------------------
163  PF_SLAM_computeObservationLikelihoodForParticle
164  ---------------------------------------------------------------*/
165 double
167  const CParticleFilter::TParticleFilterOptions& PF_options,
168  const size_t particleIndexForMap, const CSensoryFrame& observation,
169  const CPose3D& x) const
170 {
171  MRPT_UNUSED_PARAM(PF_options);
172  ASSERT_(
173  options.metricMap || particleIndexForMap < options.metricMaps.size());
174 
175  CMetricMap* map =
176  (options.metricMap) ? options.metricMap : // All particles, one map
177  options.metricMaps[particleIndexForMap]; // One map per particle
178 
179  // For each observation:
180  double ret = 1;
181  for (const auto& it : observation)
182  ret += map->computeObservationLikelihood(
183  it.get(), x); // Compute the likelihood:
184 
185  // Done!
186  return ret;
187 }
188 
189 // Specialization for my kind of particles:
192  TPose2D* particleData, const TPose3D& newPose) const
193 {
194  *particleData = TPose2D(newPose);
195 }
196 
198  CParticleList& old_particles, const vector<TPose3D>& newParticles,
199  const vector<double>& newParticlesWeight,
200  const vector<size_t>& newParticlesDerivedFromIdx) const
201 {
202  MRPT_UNUSED_PARAM(newParticlesDerivedFromIdx);
204  size_t(newParticlesWeight.size()), size_t(newParticles.size()));
205 
206  // ---------------------------------------------------------------------------------
207  // Substitute old by new particle set:
208  // Old are in "m_particles"
209  // New are in "newParticles",
210  // "newParticlesWeight","newParticlesDerivedFromIdx"
211  // ---------------------------------------------------------------------------------
212  // Free old m_particles: not needed since "d" is now a smart ptr
213 
214  // Copy into "m_particles"
215  const size_t N = newParticles.size();
216  old_particles.resize(N);
217  for (size_t i = 0; i < N; i++)
218  {
219  old_particles[i].log_w = newParticlesWeight[i];
220  old_particles[i].d = TPose2D(newParticles[i]);
221  }
222 }
223 
225  COccupancyGridMap2D* theMap, const double freeCellsThreshold,
226  const int particlesCount, const double x_min, const double x_max,
227  const double y_min, const double y_max, const double phi_min,
228  const double phi_max)
229 {
230  MRPT_START
231 
232  ASSERT_(theMap != nullptr);
233  int sizeX = theMap->getSizeX();
234  int sizeY = theMap->getSizeY();
235  double gridRes = theMap->getResolution();
236  std::vector<double> freeCells_x, freeCells_y;
237  size_t nFreeCells;
238  unsigned int xIdx1, xIdx2;
239  unsigned int yIdx1, yIdx2;
240 
241  freeCells_x.reserve(sizeX * sizeY);
242  freeCells_y.reserve(sizeX * sizeY);
243 
244  if (x_min > theMap->getXMin())
245  xIdx1 = max(0, theMap->x2idx(x_min));
246  else
247  xIdx1 = 0;
248  if (x_max < theMap->getXMax())
249  xIdx2 = min(sizeX - 1, theMap->x2idx(x_max));
250  else
251  xIdx2 = sizeX - 1;
252  if (y_min > theMap->getYMin())
253  yIdx1 = max(0, theMap->y2idx(y_min));
254  else
255  yIdx1 = 0;
256  if (y_max < theMap->getYMax())
257  yIdx2 = min(sizeY - 1, theMap->y2idx(y_max));
258  else
259  yIdx2 = sizeY - 1;
260 
261  for (unsigned int x = xIdx1; x <= xIdx2; x++)
262  for (unsigned int y = yIdx1; y <= yIdx2; y++)
263  if (theMap->getCell(x, y) >= freeCellsThreshold)
264  {
265  freeCells_x.push_back(theMap->idx2x(x));
266  freeCells_y.push_back(theMap->idx2y(y));
267  }
268 
269  nFreeCells = freeCells_x.size();
270 
271  // Assure that map is not fully occupied!
272  ASSERT_(nFreeCells);
273 
274  if (particlesCount > 0) m_particles.resize(particlesCount);
275 
276  const size_t M = m_particles.size();
277  // Generate pose m_particles:
278  for (size_t i = 0; i < M; i++)
279  {
280  int idx =
281  round(getRandomGenerator().drawUniform(0.0, nFreeCells - 1.001));
282 
283  m_particles[i].d.x =
284  freeCells_x[idx] +
285  getRandomGenerator().drawUniform(-gridRes, gridRes);
286  m_particles[i].d.y =
287  freeCells_y[idx] +
288  getRandomGenerator().drawUniform(-gridRes, gridRes);
289  m_particles[i].d.phi =
290  getRandomGenerator().drawUniform(phi_min, phi_max);
291  m_particles[i].log_w = 0;
292  }
293 
294  MRPT_END
295 }
A namespace of pseudo-random numbers generators of diferent distributions.
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...
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
#define MRPT_START
Definition: exceptions.h:282
#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.
double x
X,Y coordinates.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:108
~CMonteCarloLocalization2D() override
Destructor.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
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...
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
double x
X,Y,Z, coords.
Option set for KLD algorithm.
Definition: TKLDParams.h:17
TMonteCarloLocalizationParams options
MCL parameters.
STL namespace.
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
mrpt::maps::TMetricMapList metricMaps
[update stage] Alternative way (if metricMap==nullptr): A metric map is supplied for each particle: T...
double yaw
Yaw coordinate (rotation angle over Z axis).
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.
void PF_SLAM_implementation_custom_update_particle_with_new_pose(mrpt::math::TPose2D *particleData, const mrpt::math::TPose3D &newPose) const override
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:161
This base provides a set of functions for maths stuff.
float getXMin() const
Returns the "x" coordinate of left side of grid map.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:178
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
This namespace contains representation of robot actions and observations.
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
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:31
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
for(int w=2;w< 12;w++)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Auxiliary structure used in KLD-sampling in particle filters.
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.
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:53
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
The configuration of a particle filter.
#define MRPT_END
Definition: exceptions.h:286
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Lightweight 2D pose.
float idx2y(const size_t cy) const
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...
GLenum GLint GLint y
Definition: glext.h:3542
GLenum GLint x
Definition: glext.h:3542
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
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...
int x2idx(float x) const
Transform a coordinate value into a cell index.
double phi
Orientation (rads)
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.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019