Main MRPT website > C++ reference for MRPT 1.5.7
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-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 #include <mrpt/obs/CSensoryFrame.h>
14 
15 #include <mrpt/math/utils.h>
16 #include <mrpt/utils/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::utils;
25 using namespace mrpt::obs;
26 using namespace mrpt::maps;
27 
29 
30 namespace mrpt
31 {
32  namespace slam
33  {
34  /** Fills out a "TPoseBin3D" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options". */
35  template <>
38  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 = round( currentParticleValue->pitch() / opts.KLD_binSize_PHI );
62  outBin.roll = round( currentParticleValue->roll() / opts.KLD_binSize_PHI );
63  }
64  }
65  }
66 }
67 
69 
70 using namespace mrpt::slam;
71 
72 /*---------------------------------------------------------------
73  ctor
74  ---------------------------------------------------------------*/
75 // Passing a "this" pointer at this moment is not a problem since it will be NOT access until the object is fully initialized
76 CMonteCarloLocalization3D::CMonteCarloLocalization3D( size_t M ) :
78 {
79  this->setLoggerName("CMonteCarloLocalization3D");
80 }
81 
82 /*---------------------------------------------------------------
83  Dtor
84  ---------------------------------------------------------------*/
86 {
87 }
88 
89 
90 /*---------------------------------------------------------------
91  getLastPose
92  ---------------------------------------------------------------*/
93  TPose3D CMonteCarloLocalization3D::getLastPose(const size_t i, bool &is_valid_pose) const
94 {
95  if (i>=m_particles.size()) THROW_EXCEPTION("Particle index out of bounds!");
96  is_valid_pose = true;
97  ASSERTDEB_(m_particles[i].d!=NULL)
98  return TPose3D(*m_particles[i].d);
99 }
100 
101 
102 
103 /*---------------------------------------------------------------
104 
105  prediction_and_update_pfStandardProposal
106 
107  ---------------------------------------------------------------*/
109  const mrpt::obs::CActionCollection * actions,
110  const mrpt::obs::CSensoryFrame * sf,
112 {
113  MRPT_START
114 
115  if (sf)
116  { // A map MUST be supplied!
118  if (!options.metricMap)
119  ASSERT_(options.metricMaps.size() == m_particles.size() )
120  }
121 
122  PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin3D>(actions, sf, PF_options,options.KLD_params);
123 
124  MRPT_END
125 }
126 
127 /*---------------------------------------------------------------
128 
129  prediction_and_update_pfAuxiliaryPFStandard
130 
131  ---------------------------------------------------------------*/
133  const mrpt::obs::CActionCollection * actions,
134  const mrpt::obs::CSensoryFrame * sf,
136 {
137  MRPT_START
138 
139  if (sf)
140  { // A map MUST be supplied!
142  if (!options.metricMap)
143  ASSERT_(options.metricMaps.size() == m_particles.size() )
144  }
145 
146  PF_SLAM_implementation_pfAuxiliaryPFStandard<mrpt::slam::detail::TPoseBin3D>(actions, sf, PF_options,options.KLD_params);
147 
148  MRPT_END
149 }
150 
151 
152 /*---------------------------------------------------------------
153 
154  prediction_and_update_pfAuxiliaryPFOptimal
155 
156  ---------------------------------------------------------------*/
158  const mrpt::obs::CActionCollection * actions,
159  const mrpt::obs::CSensoryFrame * sf,
161 {
162  MRPT_START
163 
164  if (sf)
165  { // A map MUST be supplied!
167  if (!options.metricMap)
168  ASSERT_(options.metricMaps.size() == m_particles.size() )
169  }
170 
171  PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin3D>(actions, sf, PF_options,options.KLD_params);
172 
173  MRPT_END
174 }
175 
176 
177 /*---------------------------------------------------------------
178  PF_SLAM_computeObservationLikelihoodForParticle
179  ---------------------------------------------------------------*/
181  const CParticleFilter::TParticleFilterOptions &PF_options,
182  const size_t particleIndexForMap,
183  const CSensoryFrame &observation,
184  const CPose3D &x ) const
185 {
186  MRPT_UNUSED_PARAM(PF_options);
187  ASSERT_( options.metricMap || particleIndexForMap<options.metricMaps.size() )
188 
189  CMetricMap *map = (options.metricMap) ?
190  options.metricMap : // All particles, one map
191  options.metricMaps[particleIndexForMap]; // One map per particle
192 
193  // For each observation:
194  double ret = 1;
195  for (CSensoryFrame::const_iterator it=observation.begin();it!=observation.end();++it)
196  ret += map->computeObservationLikelihood( it->pointer(), x ); // Compute the likelihood:
197 
198  // Done!
199  return ret;
200 }
201 
202 // Specialization for my kind of particles:
204  CPose3D *particleData,
205  const TPose3D &newPose) const
206 {
207  *particleData = CPose3D( newPose );
208 }
209 
210 
212  CParticleList &old_particles,
213  const vector<TPose3D> &newParticles,
214  const vector<double> &newParticlesWeight,
215  const vector<size_t> &newParticlesDerivedFromIdx ) const
216 {
217  MRPT_UNUSED_PARAM(newParticlesDerivedFromIdx);
218  ASSERT_(size_t(newParticlesWeight.size())==newParticles.size())
219 
220  // ---------------------------------------------------------------------------------
221  // Substitute old by new particle set:
222  // Old are in "m_particles"
223  // New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
224  // ---------------------------------------------------------------------------------
225  // Free old m_particles (automatically done via smart ptr)
226 
227  // Copy into "m_particles"
228  const size_t N = newParticles.size();
229  old_particles.resize(N);
230  for (size_t i=0;i<N;i++)
231  {
232  old_particles[i].log_w = newParticlesWeight[i];
233  old_particles[i].d.reset(new CPose3D(newParticles[i]));
234  }
235 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
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...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
double roll
Roll coordinate (rotation angle over X coordinate).
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
#define THROW_EXCEPTION(msg)
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
Option set for KLD algorithm.
Definition: TKLDParams.h:22
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...
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==NULL): A metric map is supplied for each particle: Ther...
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.
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: ...
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.
CParticleList m_particles
The array of particles.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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...
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
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...
Definition: CPoint.h:17
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
double pitch
Pitch coordinate (rotation angle over Y axis).
Auxiliary structure used in KLD-sampling in particle filters.
#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 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
#define ASSERT_(f)
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
void PF_SLAM_implementation_custom_update_particle_with_new_pose(CParticleDataContent *particleData, const mrpt::math::TPose3D &newPose) const
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: ...
double z
X,Y,Z, coords.
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 NULL) a new pose appende...



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