MRPT  2.0.4
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::slam::PF_implementation::m_pfAuxiliaryPFStandard_estimatedProb
mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
Definition: PF_implementations_data.h:57
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:39
CPoseRandomSampler.h
mrpt::slam::PF_implementation::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::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:17
mrpt::slam::PF_implementation::PF_SLAM_computeObservationLikelihoodForParticle
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::slam::PF_implementation::PF_SLAM_implementation_replaceByNewParticleSet
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.
Definition: PF_implementations_data.h:177
mrpt::slam::PF_implementation::m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed
std::vector< bool > m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed
Definition: PF_implementations_data.h:63
CParticleFilterData.h
mrpt::bayes::particle_storage_mode
particle_storage_mode
use for CProbabilityParticle
Definition: CProbabilityParticle.h:18
mrpt::slam::PF_implementation::PF_SLAM_particlesEvaluator_AuxPFOptimal
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)
CPose3DPDFGaussian.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
mrpt::slam::PF_implementation::m_movementDrawer
mrpt::poses::CPoseRandomSampler m_movementDrawer
Used in al PF implementations.
Definition: PF_implementations_data.h:53
mrpt::slam::PF_implementation::PF_SLAM_particlesEvaluator_AuxPFStandard
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.
Definition: PF_implementations.h:503
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: CActionRobotMovement2D.h:30
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::slam::PF_implementation::m_accumRobotMovement2DIsValid
bool m_accumRobotMovement2DIsValid
Definition: PF_implementations_data.h:47
mrpt::slam::PF_implementation::PF_implementation
PF_implementation()
Definition: PF_implementations_data.h:40
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::slam::PF_implementation::m_accumRobotMovement2D
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement2D
Definition: PF_implementations_data.h:46
CActionRobotMovement2D.h
mrpt::bayes::CParticleFilterCapable
This virtual class defines the interface that any particles based PDF class must implement in order t...
Definition: CParticleFilterCapable.h:31
TPose3D.h
COutputLogger.h
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::slam::PF_implementation
A set of common data shared by PF implementations for both SLAM and localization.
Definition: PF_implementations_data.h:37
mrpt::bayes::CParticleFilterData
This template class declares the array of particles and its internal data, managing some memory-relat...
Definition: CParticleFilterData.h:184
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::slam::PF_implementation::PF_SLAM_implementation_gatherActionsCheckBothActObs
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,...
Definition: PF_implementations.h:46
CParticleFilterCapable.h
mrpt::slam::PF_implementation::PF_SLAM_implementation_doWeHaveValidObservations
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
Definition: PF_implementations_data.h:260
mrpt::math::CVectorDynamic< double >
mrpt::system::COutputLogger
Versatile class for consistent logging and management of output messages.
Definition: system/COutputLogger.h:117
mrpt::poses::CPoseRandomSampler
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
Definition: CPoseRandomSampler.h:42
CPose3D.h
TKLDParams.h
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfAuxiliaryPFOptimal
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...
Definition: PF_implementations.h:155
mrpt::slam::PF_implementation::PF_SLAM_implementation_skipRobotMovement
virtual bool PF_SLAM_implementation_skipRobotMovement() const
Make a specialization if needed, eg.
Definition: PF_implementations_data.h:269
mrpt::slam::PF_implementation::PF_SLAM_implementation_custom_update_particle_with_new_pose
virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(PARTICLE_TYPE *particleData, const mrpt::math::TPose3D &newPose) const =0
mrpt::slam::PF_implementation::m_pfAuxiliaryPFOptimal_estimatedProb
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition: PF_implementations_data.h:55
mrpt::slam::KLF_loadBinFromParticle
void KLF_loadBinFromParticle(BINTYPE &outBin, const TKLDParams &opts, const PARTICLETYPE *currentParticleValue=nullptr, const mrpt::math::TPose3D *newPoseToBeInserted=nullptr)
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal
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...
Definition: PF_implementations.h:606
mrpt::bayes::particle_storage_mode::POINTER
@ POINTER
mrpt::slam::PF_implementation::m_pfAuxiliaryPFOptimal_maxLikDrawnMovement
std::vector< mrpt::math::TPose3D > m_pfAuxiliaryPFOptimal_maxLikDrawnMovement
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition: PF_implementations_data.h:62
mrpt::slam::PF_implementation::getLastPose
virtual mrpt::math::TPose3D getLastPose(const size_t i, bool &is_valid_pose) const =0
Return the last robot pose in the i'th particle; is_valid_pose will be false if there is no such last...
mrpt::slam::PF_implementation::m_accumRobotMovement3DIsValid
bool m_accumRobotMovement3DIsValid
Definition: PF_implementations_data.h:49
mrpt::slam::PF_implementation::m_pfAuxiliaryPFOptimal_maxLikelihood
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition: PF_implementations_data.h:59
mrpt::slam::PF_implementation::PF_SLAM_aux_perform_one_rejection_sampling_step
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)
Definition: PF_implementations.h:1025
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:102
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfStandardProposal
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,...
Definition: PF_implementations.h:179
mrpt::system::COutputLogger::COutputLogger
COutputLogger()
Default class constructor.
Definition: COutputLogger.cpp:70
mrpt::slam::PF_implementation::m_accumRobotMovement3D
mrpt::poses::CPose3DPDFGaussian m_accumRobotMovement3D
Definition: PF_implementations_data.h:48



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020