MRPT  2.0.1
PF_implementations_data.h
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 #pragma once
10 
13 #include <mrpt/math/TPose3D.h>
15 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/slam/TKLDParams.h>
20 
21 namespace mrpt::slam
22 {
23 // Frwd decl:
24 template <class PARTICLETYPE, class BINTYPE>
26  BINTYPE& outBin, const TKLDParams& opts,
27  const PARTICLETYPE* currentParticleValue = nullptr,
28  const mrpt::math::TPose3D* newPoseToBeInserted = nullptr);
29 
30 /** A set of common data shared by PF implementations for both SLAM and
31  * localization
32  * \ingroup mrpt_slam_grp
33  */
34 template <
35  class PARTICLE_TYPE, class MYSELF,
38 {
39  public:
40  PF_implementation() : mrpt::system::COutputLogger("PF_implementation") {}
41 
42  protected:
43  /** \name Data members and methods used by generic PF implementations
44  @{ */
45 
50 
51  /** Used in al PF implementations. \sa
52  * PF_SLAM_implementation_gatherActionsCheckBothActObs */
54  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
56  /** Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm. */
58  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
60  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
61  mutable std::vector<mrpt::math::TPose3D>
64 
65  /** Compute w[i]*p(z_t | mu_t^i), with mu_t^i being
66  * the mean of the new robot pose
67  *
68  * \param action MUST be a "const CPose3D*"
69  * \param observation MUST be a "const CSensoryFrame*"
70  */
71  template <class BINTYPE> // Template arg. actually not used, just to allow
72  // giving the definition in another file later on
75  const mrpt::bayes::CParticleFilterCapable* obj, size_t index,
76  const void* action, const void* observation);
77 
78  template <class BINTYPE> // Template arg. actually not used, just to allow
79  // giving the definition in another file later on
82  const mrpt::bayes::CParticleFilterCapable* obj, size_t index,
83  const void* action, const void* observation);
84 
85  /** @} */
86 
87  /** \name The generic PF implementations for localization & SLAM.
88  @{ */
89 
90  /** A generic implementation of the PF method
91  * "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with
92  * rejection sampling approximation),
93  * common to both localization and mapping.
94  *
95  * - BINTYPE: TPoseBin or whatever to discretize the sample space for
96  * KLD-sampling.
97  *
98  * This method implements optimal sampling with a rejection sampling-based
99  * approximation of the true posterior.
100  * For details, see the papers:
101  *
102  * J.L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal,
103  * "An Optimal Filtering Algorithm for Non-Parametric Observation Models
104  * in
105  * Robot Localization," in Proc. IEEE International Conference on
106  * Robotics
107  * and Automation (ICRA'08), 2008, pp. 461-466.
108  */
109  template <class BINTYPE>
111  const mrpt::obs::CActionCollection* actions,
112  const mrpt::obs::CSensoryFrame* sf,
114  const TKLDParams& KLD_options);
115 
116  /** A generic implementation of the PF method
117  * "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter
118  * with the standard proposal),
119  * common to both localization and mapping.
120  *
121  * - BINTYPE: TPoseBin or whatever to discretize the sample space for
122  * KLD-sampling.
123  *
124  * This method is described in the paper:
125  * Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary
126  * Particle Filters".
127  * Journal of the American Statistical Association 94 (446): 590-591.
128  * doi:10.2307/2670179.
129  *
130  */
131  template <class BINTYPE>
133  const mrpt::obs::CActionCollection* actions,
134  const mrpt::obs::CSensoryFrame* sf,
136  const TKLDParams& KLD_options);
137 
138  /** A generic implementation of the PF method "pfStandardProposal" (standard
139  * proposal distribution, that is, a simple SIS particle filter),
140  * common to both localization and mapping.
141  *
142  * - BINTYPE: TPoseBin or whatever to discretize the sample space for
143  * KLD-sampling.
144  */
145  template <class BINTYPE>
147  const mrpt::obs::CActionCollection* actions,
148  const mrpt::obs::CSensoryFrame* sf,
150  const TKLDParams& KLD_options);
151 
152  /** @} */
153 
154  public:
155  /** \name Virtual methods that the PF_implementations assume exist.
156  @{ */
157 
158  /** Return the last robot pose in the i'th particle; is_valid_pose will be
159  * false if there is no such last pose.
160  * \exception std::exception on out-of-range particle index */
162  const size_t i, bool& is_valid_pose) const = 0;
163 
165  PARTICLE_TYPE* particleData,
166  const mrpt::math::TPose3D& newPose) const = 0;
167 
168  /** This is the default algorithm to efficiently replace one old set of
169  * samples by another new set.
170  * The method uses pointers to make fast copies the first time each
171  * particle is duplicated, then
172  * makes real copies for the next ones.
173  *
174  * Note that more efficient specializations might exist for specific
175  * particle data structs.
176  */
179  PARTICLE_TYPE, STORAGE>::CParticleList& old_particles,
180  const std::vector<mrpt::math::TPose3D>& newParticles,
181  const std::vector<double>& newParticlesWeight,
182  const std::vector<size_t>& newParticlesDerivedFromIdx) const
183  {
184  // ---------------------------------------------------------------------------------
185  // Substitute old by new particle set:
186  // Old are in "m_particles"
187  // New are in "newParticles",
188  // "newParticlesWeight","newParticlesDerivedFromIdx"
189  // ---------------------------------------------------------------------------------
190  const size_t N = newParticles.size();
191  std::remove_reference_t<decltype(old_particles)> newParticlesArray(N);
192  if constexpr (STORAGE == mrpt::bayes::particle_storage_mode::POINTER)
193  {
194  // For efficiency, just copy the "CParticleData" from the old
195  // particle into the new one, but this can be done only once:
196  const auto N_old = old_particles.size();
197  std::vector<bool> oldParticleAlreadyCopied(N_old, false);
198  std::vector<PARTICLE_TYPE*> oldParticleFirstCopies(N_old, nullptr);
199 
200  auto newPartIt = newParticlesArray.begin();
201  for (size_t i = 0; newPartIt != newParticlesArray.end();
202  ++newPartIt, ++i)
203  {
204  // The weight:
205  newPartIt->log_w = newParticlesWeight[i];
206 
207  // The data (CParticleData):
208  PARTICLE_TYPE* newPartData;
209  const size_t i_in_old = newParticlesDerivedFromIdx[i];
210  if (!oldParticleAlreadyCopied[i_in_old])
211  {
212  // The first copy of this old particle:
213  newPartData = old_particles[i_in_old].d.release();
214  oldParticleAlreadyCopied[i_in_old] = true;
215  oldParticleFirstCopies[i_in_old] = newPartData;
216  }
217  else
218  {
219  // Make a copy:
220  ASSERT_(oldParticleFirstCopies[i_in_old]);
221  newPartData =
222  new PARTICLE_TYPE(*oldParticleFirstCopies[i_in_old]);
223  }
224 
225  newPartIt->d.reset(newPartData);
226  } // end for "newPartIt"
227 
228  // Now add the new robot pose to the paths:
229  // (this MUST be done after the above loop, separately):
230  // Update the particle with the new pose: this part is
231  // caller-dependant and must be implemented there:
232  newPartIt = newParticlesArray.begin();
233  for (size_t i = 0; i < N; ++newPartIt, ++i)
235  newPartIt->d.get(), newParticles[i]);
236  }
237  else
238  {
239  // Particles as values:
240 
241  auto newPartIt = newParticlesArray.begin();
242  for (size_t i = 0; newPartIt != newParticlesArray.end();
243  ++newPartIt, ++i)
244  {
245  newPartIt->log_w = newParticlesWeight[i];
246  const size_t i_in_old = newParticlesDerivedFromIdx[i];
247  newPartIt->d = old_particles[i_in_old].d;
248  }
249 
250  // Now add the new robot pose to the paths:
251  newPartIt = newParticlesArray.begin();
252  for (size_t i = 0; i < N; ++newPartIt, ++i)
254  &newPartIt->d, newParticles[i]);
255  }
256  // Move to "m_particles"
257  old_particles = std::move(newParticlesArray);
258  } // end of PF_SLAM_implementation_replaceByNewParticleSet
259 
261  [[maybe_unused]] const typename mrpt::bayes::CParticleFilterData<
262  PARTICLE_TYPE, STORAGE>::CParticleList& particles,
263  [[maybe_unused]] const mrpt::obs::CSensoryFrame* sf) const
264  {
265  return true; // By default, always process the SFs.
266  }
267 
268  /** Make a specialization if needed, eg. in the first step in SLAM. */
270  {
271  return false; // By default, always allow the robot to move!
272  }
273 
274  /** Evaluate the observation likelihood for one particle at a given location
275  */
278  const size_t particleIndexForMap,
279  const mrpt::obs::CSensoryFrame& observation,
280  const mrpt::poses::CPose3D& x) const = 0;
281 
282  /** @} */
283 
284  /** Auxiliary method called by PF implementations: return true if we have
285  * both action & observation,
286  * otherwise, return false AND accumulate the odometry so when we have an
287  * observation we didn't lose a thing.
288  * On return=true, the "m_movementDrawer" member is loaded and ready to
289  * draw samples of the increment of pose since last step.
290  * This method is smart enough to accumulate CActionRobotMovement2D or
291  * CActionRobotMovement3D, whatever comes in.
292  */
293  template <class BINTYPE> // Template arg. actually not used, just to allow
294  // giving the definition in another file later on
296  const mrpt::obs::CActionCollection* actions,
297  const mrpt::obs::CSensoryFrame* sf);
298 
299  private:
300  /** The shared implementation body of two PF methods: APF and Optimal-APF,
301  * depending on USE_OPTIMAL_SAMPLING */
302  template <class BINTYPE>
304  const mrpt::obs::CActionCollection* actions,
305  const mrpt::obs::CSensoryFrame* sf,
307  const TKLDParams& KLD_options, const bool USE_OPTIMAL_SAMPLING);
308 
309  template <class BINTYPE>
311  const bool USE_OPTIMAL_SAMPLING, const bool doResample,
312  const double maxMeanLik,
313  size_t k, // The particle from the old set "m_particles[]"
314  const mrpt::obs::CSensoryFrame* sf,
316  mrpt::poses::CPose3D& out_newPose, double& out_newParticleLogWeight);
317 
318 }; // end PF_implementation
319 } // namespace mrpt::slam
mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void KLF_loadBinFromParticle(BINTYPE &outBin, const TKLDParams &opts, const PARTICLETYPE *currentParticleValue=nullptr, const mrpt::math::TPose3D *newPoseToBeInserted=nullptr)
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...
Option set for KLD algorithm.
Definition: TKLDParams.h:17
virtual mrpt::math::TPose3D getLastPose(const size_t i, bool &is_valid_pose) const =0
Return the last robot pose in the i&#39;th particle; is_valid_pose will be false if there is no such last...
virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(PARTICLE_TYPE *particleData, const mrpt::math::TPose3D &newPose) const =0
void PF_SLAM_implementation_pfAuxiliaryPFOptimal(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_pfAuxiliaryPFOptimal" (optimal sampl...
Declares a class for storing a collection of robot actions.
std::vector< bool > m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed
A set of common data shared by PF implementations for both SLAM and localization. ...
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
Represents a probabilistic 2D movement of the robot mobile base.
void PF_SLAM_aux_perform_one_rejection_sampling_step(const bool USE_OPTIMAL_SAMPLING, const bool doResample, const double maxMeanLik, size_t k, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, mrpt::poses::CPose3D &out_newPose, double &out_newParticleLogWeight)
Versatile class for consistent logging and management of output messages.
mrpt::poses::CPoseRandomSampler m_movementDrawer
Used in al PF implementations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
This virtual class defines the interface that any particles based PDF class must implement in order t...
This template class declares the array of particles and its internal data, managing some memory-relat...
virtual bool PF_SLAM_implementation_doWeHaveValidObservations([[maybe_unused]] const typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE, STORAGE >::CParticleList &particles, [[maybe_unused]] const mrpt::obs::CSensoryFrame *sf) const
void PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options, const bool USE_OPTIMAL_SAMPLING)
The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPL...
virtual 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 =0
Evaluate the observation likelihood for one particle at a given location.
mrpt::poses::CPose3DPDFGaussian m_accumRobotMovement3D
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static double PF_SLAM_particlesEvaluator_AuxPFOptimal(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
void PF_SLAM_implementation_pfStandardProposal(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 "pfStandardProposal" (standard proposal distribution...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
static double PF_SLAM_particlesEvaluator_AuxPFStandard(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
Compute w[i]*p(z_t | mu_t^i), with mu_t^i being the mean of the new robot pose.
The configuration of a particle filter.
COutputLogger()
Default class constructor.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
std::vector< mrpt::math::TPose3D > m_pfAuxiliaryPFOptimal_maxLikDrawnMovement
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement2D
bool PF_SLAM_implementation_gatherActionsCheckBothActObs(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf)
Auxiliary method called by PF implementations: return true if we have both action & observation...
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
particle_storage_mode
use for CProbabilityParticle
virtual bool PF_SLAM_implementation_skipRobotMovement() const
Make a specialization if needed, eg.
virtual void PF_SLAM_implementation_replaceByNewParticleSet(typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE, STORAGE >::CParticleList &old_particles, const std::vector< mrpt::math::TPose3D > &newParticles, const std::vector< double > &newParticlesWeight, const std::vector< size_t > &newParticlesDerivedFromIdx) const
This is the default algorithm to efficiently replace one old set of samples by another new set...



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020