MRPT  2.0.3
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 }
mrpt::slam::detail::TPoseBin3D::pitch
int pitch
Definition: PF_aux_structs.h:75
mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandard
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...
Definition: PF_implementations.h:404
mrpt::slam::CMonteCarloLocalization3D::PF_SLAM_computeObservationLikelihoodForParticle
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.
Definition: CMonteCarloLocalization3D.cpp:170
mrpt::slam::TMonteCarloLocalizationParams::metricMaps
mrpt::maps::TMetricMapList metricMaps
[update stage] Alternative way (if metricMap==nullptr): A metric map is supplied for each particle: T...
Definition: TMonteCarloLocalizationParams.h:44
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:17
mrpt::slam::CMonteCarloLocalization3D::PF_SLAM_implementation_replaceByNewParticleSet
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
Definition: CMonteCarloLocalization3D.cpp:201
mrpt::slam::CMonteCarloLocalization3D::getLastPose
mrpt::math::TPose3D getLastPose(const size_t i, bool &is_valid_pose) const override
Return the robot pose for the i'th particle.
Definition: CMonteCarloLocalization3D.cpp:81
PF_implementations_data.h
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: CActionCollection.h:26
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
PF_implementations.h
mrpt::math::TPose3D::y
double y
Definition: TPose3D.h:32
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
mrpt::math::TPose3D::z
double z
Definition: TPose3D.h:32
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::slam::CMonteCarloLocalization3D::prediction_and_update_pfStandardProposal
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.
Definition: CMonteCarloLocalization3D.cpp:95
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::slam::KLF_loadBinFromParticle
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...
Definition: CMonteCarloLocalization3D.cpp:35
mrpt::slam::detail::TPoseBin3D::z
int z
Definition: PF_aux_structs.h:75
mrpt::slam::TKLDParams::KLD_binSize_PHI
double KLD_binSize_PHI
Definition: TKLDParams.h:31
round.h
mrpt::slam::detail::TPoseBin3D::roll
int roll
Definition: PF_aux_structs.h:75
mrpt::math::TPose3D::x
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
for
for(unsigned int i=0;i< NUM_IMGS;i++)
Definition: chessboard_stereo_camera_calib_unittest.cpp:33
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::slam::CMonteCarloLocalization3D::PF_SLAM_implementation_custom_update_particle_with_new_pose
void PF_SLAM_implementation_custom_update_particle_with_new_pose(CParticleDataContent *particleData, const mrpt::math::TPose3D &newPose) const override
Definition: CMonteCarloLocalization3D.cpp:195
mrpt::slam::detail::TPoseBin3D
Auxiliary structure used in KLD-sampling in particle filters.
Definition: PF_aux_structs.h:71
CMonteCarloLocalization3D.h
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:29
utils.h
mrpt::math::TPose3D::pitch
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
PF_aux_structs.h
CSensoryFrame.h
mrpt::slam::detail::TPoseBin3D::x
int x
Bin indices.
Definition: PF_aux_structs.h:75
mrpt::bayes::CParticleFilterData< mrpt::math::TPose3D, mrpt::bayes::particle_storage_mode::VALUE >::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:195
mrpt::slam::CMonteCarloLocalization3D::prediction_and_update_pfAuxiliaryPFOptimal
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.
Definition: CMonteCarloLocalization3D.cpp:146
mrpt::math::TPose3D::yaw
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
mrpt::slam::detail::TPoseBin3D::y
int y
Definition: PF_aux_structs.h:75
mrpt::math::TPose3D::roll
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
slam-precomp.h
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:30
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::slam::CMonteCarloLocalization3D::prediction_and_update_pfAuxiliaryPFStandard
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.
Definition: CMonteCarloLocalization3D.cpp:120
mrpt::maps::CMetricMap::computeObservationLikelihood
double computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom)
Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.
Definition: CMetricMap.cpp:170
mrpt::slam::CMonteCarloLocalization3D::options
TMonteCarloLocalizationParams options
MCL parameters.
Definition: CMonteCarloLocalization3D.h:37
mrpt::slam::TMonteCarloLocalizationParams::metricMap
mrpt::maps::CMetricMap * metricMap
[update stage] Must be set to a metric map used to estimate the likelihood of observations
Definition: TMonteCarloLocalizationParams.h:38
mrpt::maps
Definition: CBeacon.h:21
mrpt::system::COutputLogger::setLoggerName
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
Definition: COutputLogger.cpp:138
mrpt::slam::detail::TPoseBin3D::yaw
int yaw
Definition: PF_aux_structs.h:75
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:102
mrpt::slam::TKLDParams::KLD_binSize_XY
double KLD_binSize_XY
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...
Definition: TKLDParams.h:31
mrpt::slam::TMonteCarloLocalizationParams::KLD_params
TKLDParams KLD_params
Parameters for dynamic sample size, KLD method.
Definition: TMonteCarloLocalizationParams.h:47



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020