Main MRPT website > C++ reference for MRPT 1.9.9
CMonteCarloLocalization3D.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 #include <mrpt/obs/CSensoryFrame.h>
14 
15 #include <mrpt/math/utils.h>
16 #include <mrpt/core/round.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
30 {
31 namespace slam
32 {
33 /** Fills out a "TPoseBin3D" variable, given a path hypotesis and (if not set to
34  * nullptr) a new pose appended at the end, using the KLD params in "options".
35  */
36 template <>
38  mrpt::slam::detail::TPoseBin3D& outBin, const TKLDParams& opts,
39  const CMonteCarloLocalization3D::CParticleDataContent* currentParticleValue,
40  const TPose3D* newPoseToBeInserted)
41 {
42  // 3D pose approx: Use the latest pose only:
43  if (newPoseToBeInserted)
44  {
45  outBin.x = round(newPoseToBeInserted->x / opts.KLD_binSize_XY);
46  outBin.y = round(newPoseToBeInserted->y / opts.KLD_binSize_XY);
47  outBin.z = round(newPoseToBeInserted->z / opts.KLD_binSize_XY);
48 
49  outBin.yaw = round(newPoseToBeInserted->yaw / opts.KLD_binSize_PHI);
50  outBin.pitch = round(newPoseToBeInserted->pitch / opts.KLD_binSize_PHI);
51  outBin.roll = round(newPoseToBeInserted->roll / 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.z = round(currentParticleValue->z / opts.KLD_binSize_XY);
59 
60  outBin.yaw = round(currentParticleValue->yaw / opts.KLD_binSize_PHI);
61  outBin.pitch =
62  round(currentParticleValue->pitch / opts.KLD_binSize_PHI);
63  outBin.roll = round(currentParticleValue->roll / opts.KLD_binSize_PHI);
64  }
65 }
66 } // namespace slam
67 } // namespace mrpt
68 
70 
71 using namespace mrpt::slam;
72 
73 /*---------------------------------------------------------------
74  ctor
75  ---------------------------------------------------------------*/
76 // Passing a "this" pointer at this moment is not a problem since it will be NOT
77 // access until the object is fully initialized
78 CMonteCarloLocalization3D::CMonteCarloLocalization3D(size_t M)
80 {
81  this->setLoggerName("CMonteCarloLocalization3D");
82 }
83 
85  const size_t i, bool& is_valid_pose) const
86 {
87  if (i >= m_particles.size())
88  THROW_EXCEPTION("Particle index out of bounds!");
89  is_valid_pose = true;
90  return m_particles[i].d;
91 }
92 
93 /*---------------------------------------------------------------
94 
95  prediction_and_update_pfStandardProposal
96 
97  ---------------------------------------------------------------*/
99  const mrpt::obs::CActionCollection* actions,
100  const mrpt::obs::CSensoryFrame* sf,
102 {
103  MRPT_START
104 
105  if (sf)
106  { // A map MUST be supplied!
107  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
108  if (!options.metricMap)
109  ASSERT_(options.metricMaps.size() == m_particles.size());
110  }
111 
112  PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin3D>(
113  actions, sf, PF_options, options.KLD_params);
114 
115  MRPT_END
116 }
117 
118 /*---------------------------------------------------------------
119 
120  prediction_and_update_pfAuxiliaryPFStandard
121 
122  ---------------------------------------------------------------*/
124  const mrpt::obs::CActionCollection* actions,
125  const mrpt::obs::CSensoryFrame* sf,
127 {
128  MRPT_START
129 
130  if (sf)
131  { // A map MUST be supplied!
132  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
133  if (!options.metricMap)
134  ASSERT_(options.metricMaps.size() == m_particles.size());
135  }
136 
139  actions, sf, PF_options, options.KLD_params);
140 
141  MRPT_END
142 }
143 
144 /*---------------------------------------------------------------
145 
146  prediction_and_update_pfAuxiliaryPFOptimal
147 
148  ---------------------------------------------------------------*/
150  const mrpt::obs::CActionCollection* actions,
151  const mrpt::obs::CSensoryFrame* sf,
153 {
154  MRPT_START
155 
156  if (sf)
157  { // A map MUST be supplied!
158  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
159  if (!options.metricMap)
160  ASSERT_(options.metricMaps.size() == m_particles.size());
161  }
162 
163  PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin3D>(
164  actions, sf, PF_options, options.KLD_params);
165 
166  MRPT_END
167 }
168 
169 /*---------------------------------------------------------------
170  PF_SLAM_computeObservationLikelihoodForParticle
171  ---------------------------------------------------------------*/
172 double
174  [[maybe_unused]] const CParticleFilter::TParticleFilterOptions& PF_options,
175  const size_t particleIndexForMap, const CSensoryFrame& observation,
176  const CPose3D& x) const
177 {
178  ASSERT_(
179  options.metricMap || particleIndexForMap < options.metricMaps.size());
180 
181  CMetricMap* map =
182  (options.metricMap) ? options.metricMap : // All particles, one map
183  options.metricMaps[particleIndexForMap]; // One map per particle
184 
185  // For each observation:
186  double ret = 1;
187  for (CSensoryFrame::const_iterator it = observation.begin();
188  it != observation.end(); ++it)
189  ret += map->computeObservationLikelihood(
190  it->get(), x); // Compute the likelihood:
191 
192  // Done!
193  return ret;
194 }
195 
196 // Specialization for my kind of particles:
199  TPose3D* particleData, const TPose3D& newPose) const
200 {
201  *particleData = newPose;
202 }
203 
205  CParticleList& old_particles, const vector<TPose3D>& newParticles,
206  const vector<double>& newParticlesWeight,
207  [[maybe_unused]] const vector<size_t>& newParticlesDerivedFromIdx) const
208 {
209  ASSERT_(size_t(newParticlesWeight.size()) == newParticles.size());
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 (automatically done via 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 = newParticles[i];
225  }
226 }
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 MRPT_START
Definition: exceptions.h:262
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
double roll
Roll coordinate (rotation angle over X coordinate).
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:20
STL namespace.
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...
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.
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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.
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.
std::deque< CObservation::Ptr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
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:54
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:34
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...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double pitch
Pitch coordinate (rotation angle over Y axis).
Auxiliary structure used in KLD-sampling in particle filters.
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:56
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
The configuration of a particle filter.
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 MRPT_END
Definition: exceptions.h:266
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
void PF_SLAM_implementation_custom_update_particle_with_new_pose(CParticleDataContent *particleData, const mrpt::math::TPose3D &newPose) const
GLenum GLint x
Definition: glext.h:3538
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
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 class that represents a Probability Density function (PDF) of a 3D pose.
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
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:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019