MRPT  2.0.4
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-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 
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  [[maybe_unused]] const CParticleFilter::TParticleFilterOptions&
168  PF_options,
169  const size_t particleIndexForMap, const CSensoryFrame& observation,
170  const CPose3D& x) const
171 {
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, 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  [[maybe_unused]] const vector<size_t>& newParticlesDerivedFromIdx) const
201 {
203  size_t(newParticlesWeight.size()), size_t(newParticles.size()));
204 
205  // ---------------------------------------------------------------------------------
206  // Substitute old by new particle set:
207  // Old are in "m_particles"
208  // New are in "newParticles",
209  // "newParticlesWeight","newParticlesDerivedFromIdx"
210  // ---------------------------------------------------------------------------------
211  // Free old m_particles: not needed since "d" is now a smart ptr
212 
213  // Copy into "m_particles"
214  const size_t N = newParticles.size();
215  old_particles.resize(N);
216  for (size_t i = 0; i < N; i++)
217  {
218  old_particles[i].log_w = newParticlesWeight[i];
219  old_particles[i].d = TPose2D(newParticles[i]);
220  }
221 }
222 
224  COccupancyGridMap2D* theMap, const double freeCellsThreshold,
225  const int particlesCount, const double x_min, const double x_max,
226  const double y_min, const double y_max, const double phi_min,
227  const double phi_max)
228 {
229  MRPT_START
230 
231  ASSERT_(theMap != nullptr);
232  int sizeX = theMap->getSizeX();
233  int sizeY = theMap->getSizeY();
234  double gridRes = theMap->getResolution();
235  std::vector<double> freeCells_x, freeCells_y;
236  size_t nFreeCells;
237  unsigned int xIdx1, xIdx2;
238  unsigned int yIdx1, yIdx2;
239 
240  freeCells_x.reserve(sizeX * sizeY);
241  freeCells_y.reserve(sizeX * sizeY);
242 
243  if (x_min > theMap->getXMin())
244  xIdx1 = max(0, theMap->x2idx(x_min));
245  else
246  xIdx1 = 0;
247  if (x_max < theMap->getXMax())
248  xIdx2 = min(sizeX - 1, theMap->x2idx(x_max));
249  else
250  xIdx2 = sizeX - 1;
251  if (y_min > theMap->getYMin())
252  yIdx1 = max(0, theMap->y2idx(y_min));
253  else
254  yIdx1 = 0;
255  if (y_max < theMap->getYMax())
256  yIdx2 = min(sizeY - 1, theMap->y2idx(y_max));
257  else
258  yIdx2 = sizeY - 1;
259 
260  for (unsigned int x = xIdx1; x <= xIdx2; x++)
261  for (unsigned int y = yIdx1; y <= yIdx2; y++)
262  if (theMap->getCell(x, y) >= freeCellsThreshold)
263  {
264  freeCells_x.push_back(theMap->idx2x(x));
265  freeCells_y.push_back(theMap->idx2y(y));
266  }
267 
268  nFreeCells = freeCells_x.size();
269 
270  // Assure that map is not fully occupied!
271  ASSERT_(nFreeCells);
272 
273  if (particlesCount > 0) m_particles.resize(particlesCount);
274 
275  const size_t M = m_particles.size();
276  // Generate pose m_particles:
277  for (size_t i = 0; i < M; i++)
278  {
279  int idx =
280  round(getRandomGenerator().drawUniform(0.0, nFreeCells - 1.001));
281 
282  m_particles[i].d.x =
283  freeCells_x[idx] +
284  getRandomGenerator().drawUniform(-gridRes, gridRes);
285  m_particles[i].d.y =
286  freeCells_y[idx] +
287  getRandomGenerator().drawUniform(-gridRes, gridRes);
288  m_particles[i].d.phi =
289  getRandomGenerator().drawUniform(phi_min, phi_max);
290  m_particles[i].log_w = 0;
291  }
292 
293  MRPT_END
294 }
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...
float getYMin() const
Returns the "y" coordinate of top side of grid map.
#define MRPT_START
Definition: exceptions.h:241
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.
Definition: TPose2D.h:30
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
~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.
Definition: TPose3D.h:32
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).
Definition: TPose3D.h:34
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:120
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
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:137
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...
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:52
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.
#define MRPT_END
Definition: exceptions.h:245
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Lightweight 2D pose.
Definition: TPose2D.h:22
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...
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)
Definition: TPose2D.h:32
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.
for(unsigned int i=0;i< NUM_IMGS;i++)
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020