MRPT  1.9.9
CMonteCarloLocalization3D.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 
12 #include <mrpt/obs/CSensoryFrame.h>
14 
15 #include <mrpt/core/round.h>
16 #include <mrpt/math/utils.h>
18 
19 using namespace std;
20 using namespace mrpt;
21 using namespace mrpt::bayes;
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 using namespace mrpt::obs;
25 using namespace mrpt::maps;
26 
28 
29 namespace mrpt::slam
30 {
31 /** Fills out a "TPoseBin3D" variable, given a path hypotesis and (if not set to
32  * nullptr) a new pose appended at the end, using the KLD params in "options".
33  */
34 template <>
36  mrpt::slam::detail::TPoseBin3D& outBin, const TKLDParams& opts,
37  const CMonteCarloLocalization3D::CParticleDataContent* currentParticleValue,
38  const TPose3D* newPoseToBeInserted)
39 {
40  // 3D pose approx: Use the latest pose only:
41  if (newPoseToBeInserted)
42  {
43  outBin.x = round(newPoseToBeInserted->x / opts.KLD_binSize_XY);
44  outBin.y = round(newPoseToBeInserted->y / opts.KLD_binSize_XY);
45  outBin.z = round(newPoseToBeInserted->z / opts.KLD_binSize_XY);
46 
47  outBin.yaw = round(newPoseToBeInserted->yaw / opts.KLD_binSize_PHI);
48  outBin.pitch = round(newPoseToBeInserted->pitch / opts.KLD_binSize_PHI);
49  outBin.roll = round(newPoseToBeInserted->roll / opts.KLD_binSize_PHI);
50  }
51  else
52  {
53  ASSERT_(currentParticleValue);
54  outBin.x = round(currentParticleValue->x / opts.KLD_binSize_XY);
55  outBin.y = round(currentParticleValue->y / opts.KLD_binSize_XY);
56  outBin.z = round(currentParticleValue->z / opts.KLD_binSize_XY);
57 
58  outBin.yaw = round(currentParticleValue->yaw / opts.KLD_binSize_PHI);
59  outBin.pitch =
60  round(currentParticleValue->pitch / opts.KLD_binSize_PHI);
61  outBin.roll = round(currentParticleValue->roll / opts.KLD_binSize_PHI);
62  }
63 }
64 } // namespace mrpt::slam
65 
67 
68 using namespace mrpt::slam;
69 
70 /*---------------------------------------------------------------
71  ctor
72  ---------------------------------------------------------------*/
73 // Passing a "this" pointer at this moment is not a problem since it will be NOT
74 // access until the object is fully initialized
75 CMonteCarloLocalization3D::CMonteCarloLocalization3D(size_t M)
77 {
78  this->setLoggerName("CMonteCarloLocalization3D");
79 }
80 
82  const size_t i, bool& is_valid_pose) const
83 {
84  if (i >= m_particles.size())
85  THROW_EXCEPTION("Particle index out of bounds!");
86  is_valid_pose = true;
87  return m_particles[i].d;
88 }
89 
90 /*---------------------------------------------------------------
91 
92  prediction_and_update_pfStandardProposal
93 
94  ---------------------------------------------------------------*/
96  const mrpt::obs::CActionCollection* actions,
97  const mrpt::obs::CSensoryFrame* sf,
99 {
100  MRPT_START
101 
102  if (sf)
103  { // A map MUST be supplied!
104  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
105  if (!options.metricMap)
106  ASSERT_(options.metricMaps.size() == m_particles.size());
107  }
108 
109  PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin3D>(
110  actions, sf, PF_options, options.KLD_params);
111 
112  MRPT_END
113 }
114 
115 /*---------------------------------------------------------------
116 
117  prediction_and_update_pfAuxiliaryPFStandard
118 
119  ---------------------------------------------------------------*/
121  const mrpt::obs::CActionCollection* actions,
122  const mrpt::obs::CSensoryFrame* sf,
124 {
125  MRPT_START
126 
127  if (sf)
128  { // A map MUST be supplied!
129  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
130  if (!options.metricMap)
131  ASSERT_(options.metricMaps.size() == m_particles.size());
132  }
133 
136  actions, sf, PF_options, options.KLD_params);
137 
138  MRPT_END
139 }
140 
141 /*---------------------------------------------------------------
142 
143  prediction_and_update_pfAuxiliaryPFOptimal
144 
145  ---------------------------------------------------------------*/
147  const mrpt::obs::CActionCollection* actions,
148  const mrpt::obs::CSensoryFrame* sf,
150 {
151  MRPT_START
152 
153  if (sf)
154  { // A map MUST be supplied!
155  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
156  if (!options.metricMap)
157  ASSERT_(options.metricMaps.size() == m_particles.size());
158  }
159 
160  PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin3D>(
161  actions, sf, PF_options, options.KLD_params);
162 
163  MRPT_END
164 }
165 
166 /*---------------------------------------------------------------
167  PF_SLAM_computeObservationLikelihoodForParticle
168  ---------------------------------------------------------------*/
169 double
171  [[maybe_unused]] const CParticleFilter::TParticleFilterOptions&
172  PF_options,
173  const size_t particleIndexForMap, const CSensoryFrame& observation,
174  const CPose3D& x) const
175 {
176  ASSERT_(
177  options.metricMap || particleIndexForMap < options.metricMaps.size());
178 
179  CMetricMap* map =
180  (options.metricMap) ? options.metricMap : // All particles, one map
181  options.metricMaps[particleIndexForMap]; // One map per particle
182 
183  // For each observation:
184  double ret = 1;
185  for (const auto& it : observation)
186  ret += map->computeObservationLikelihood(
187  *it, x); // Compute the likelihood:
188 
189  // Done!
190  return ret;
191 }
192 
193 // Specialization for my kind of particles:
196  TPose3D* particleData, const TPose3D& newPose) const
197 {
198  *particleData = newPose;
199 }
200 
202  CParticleList& old_particles, const vector<TPose3D>& newParticles,
203  const vector<double>& newParticlesWeight,
204  [[maybe_unused]] const vector<size_t>& newParticlesDerivedFromIdx) const
205 {
206  ASSERT_(size_t(newParticlesWeight.size()) == newParticles.size());
207  // ---------------------------------------------------------------------------------
208  // Substitute old by new particle set:
209  // Old are in "m_particles"
210  // New are in "newParticles",
211  // "newParticlesWeight","newParticlesDerivedFromIdx"
212  // ---------------------------------------------------------------------------------
213  // Free old m_particles (automatically done via smart ptr)
214 
215  // Copy into "m_particles"
216  const size_t N = newParticles.size();
217  old_particles.resize(N);
218  for (size_t i = 0; i < N; i++)
219  {
220  old_particles[i].log_w = newParticlesWeight[i];
221  old_particles[i].d = newParticles[i];
222  }
223 }
#define MRPT_START
Definition: exceptions.h:241
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
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
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...
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
STL namespace.
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 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...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
TMonteCarloLocalizationParams options
MCL parameters.
This base provides a set of functions for maths stuff.
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
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...
Definition: CSensoryFrame.h:51
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
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
Auxiliary structure used in KLD-sampling in particle filters.
void PF_SLAM_implementation_custom_update_particle_with_new_pose(CParticleDataContent *particleData, const mrpt::math::TPose3D &newPose) const override
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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
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...
mrpt::math::TPose3D getLastPose(const size_t i, bool &is_valid_pose) const override
Return the robot pose for the i&#39;th particle.
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.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
for(unsigned int i=0;i< NUM_IMGS;i++)
void KLF_loadBinFromParticle(mrpt::slam::detail::TPoseBin3D &outBin, const TKLDParams &opts, const CMonteCarloLocalization3D::CParticleDataContent *currentParticleValue, const TPose3D *newPoseToBeInserted)
Fills out a "TPoseBin3D" variable, given a path hypotesis and (if not set to nullptr) a new pose appe...
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 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020