Main MRPT website > C++ reference for MRPT 1.9.9
PF_implementations_data.h
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 #ifndef PF_implementations_data_H
10 #define PF_implementations_data_H
11 
16 #include <mrpt/poses/CPose3D.h>
19 #include <mrpt/slam/TKLDParams.h>
21 
22 namespace mrpt
23 {
24 namespace slam
25 {
26 // Frwd decl:
27 template <class PARTICLETYPE, class BINTYPE>
29  BINTYPE& outBin, const TKLDParams& opts,
30  const PARTICLETYPE* currentParticleValue = nullptr,
31  const mrpt::math::TPose3D* newPoseToBeInserted = nullptr);
32 
33 /** A set of common data shared by PF implementations for both SLAM and
34  * localization
35  * \ingroup mrpt_slam_grp
36  */
37 template <
38  class PARTICLE_TYPE, class MYSELF,
41 {
42  public:
44  : mrpt::system::COutputLogger("PF_implementation"),
47  {
48  }
49 
50  protected:
51  /** \name Data members and methods used by generic PF implementations
52  @{ */
53 
58 
59  /** Used in al PF implementations. \sa
60  * PF_SLAM_implementation_gatherActionsCheckBothActObs */
62  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
64  /** Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm. */
66  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
68  /** Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. */
69  mutable std::vector<mrpt::math::TPose3D>
72 
73  /** Compute w[i]*p(z_t | mu_t^i), with mu_t^i being
74  * the mean of the new robot pose
75  *
76  * \param action MUST be a "const CPose3D*"
77  * \param observation MUST be a "const CSensoryFrame*"
78  */
79  template <class BINTYPE> // Template arg. actually not used, just to allow
80  // giving the definition in another file later on
84  const void* action, const void* observation);
85 
86  template <class BINTYPE> // Template arg. actually not used, just to allow
87  // giving the definition in another file later on
91  const void* action, const void* observation);
92 
93  /** @} */
94 
95  /** \name The generic PF implementations for localization & SLAM.
96  @{ */
97 
98  /** A generic implementation of the PF method
99  * "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with
100  * rejection sampling approximation),
101  * common to both localization and mapping.
102  *
103  * - BINTYPE: TPoseBin or whatever to discretize the sample space for
104  * KLD-sampling.
105  *
106  * This method implements optimal sampling with a rejection sampling-based
107  * approximation of the true posterior.
108  * For details, see the papers:
109  *
110  * J.L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal,
111  * "An Optimal Filtering Algorithm for Non-Parametric Observation Models
112  * in
113  * Robot Localization," in Proc. IEEE International Conference on
114  * Robotics
115  * and Automation (ICRA'08), 2008, pp. 461-466.
116  */
117  template <class BINTYPE>
119  const mrpt::obs::CActionCollection* actions,
120  const mrpt::obs::CSensoryFrame* sf,
122  const TKLDParams& KLD_options);
123 
124  /** A generic implementation of the PF method
125  * "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter
126  * with the standard proposal),
127  * common to both localization and mapping.
128  *
129  * - BINTYPE: TPoseBin or whatever to discretize the sample space for
130  * KLD-sampling.
131  *
132  * This method is described in the paper:
133  * Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary
134  * Particle Filters".
135  * Journal of the American Statistical Association 94 (446): 590-591.
136  * doi:10.2307/2670179.
137  *
138  */
139  template <class BINTYPE>
141  const mrpt::obs::CActionCollection* actions,
142  const mrpt::obs::CSensoryFrame* sf,
144  const TKLDParams& KLD_options);
145 
146  /** A generic implementation of the PF method "pfStandardProposal" (standard
147  * proposal distribution, that is, a simple SIS particle filter),
148  * common to both localization and mapping.
149  *
150  * - BINTYPE: TPoseBin or whatever to discretize the sample space for
151  * KLD-sampling.
152  */
153  template <class BINTYPE>
155  const mrpt::obs::CActionCollection* actions,
156  const mrpt::obs::CSensoryFrame* sf,
158  const TKLDParams& KLD_options);
159 
160  /** @} */
161 
162  public:
163  /** \name Virtual methods that the PF_implementations assume exist.
164  @{ */
165 
166  /** Return the last robot pose in the i'th particle; is_valid_pose will be
167  * false if there is no such last pose.
168  * \exception std::exception on out-of-range particle index */
170  const size_t i, bool& is_valid_pose) const = 0;
171 
173  PARTICLE_TYPE* particleData,
174  const mrpt::math::TPose3D& newPose) const = 0;
175 
176  /** This is the default algorithm to efficiently replace one old set of
177  * samples by another new set.
178  * The method uses pointers to make fast copies the first time each
179  * particle is duplicated, then
180  * makes real copies for the next ones.
181  *
182  * Note that more efficient specializations might exist for specific
183  * particle data structs.
184  */
187  PARTICLE_TYPE, STORAGE>::CParticleList& old_particles,
188  const std::vector<mrpt::math::TPose3D>& newParticles,
189  const std::vector<double>& newParticlesWeight,
190  const std::vector<size_t>& newParticlesDerivedFromIdx) const
191  {
192  // ---------------------------------------------------------------------------------
193  // Substitute old by new particle set:
194  // Old are in "m_particles"
195  // New are in "newParticles",
196  // "newParticlesWeight","newParticlesDerivedFromIdx"
197  // ---------------------------------------------------------------------------------
198  const size_t N = newParticles.size();
199  std::remove_reference_t<decltype(old_particles)> newParticlesArray(N);
200  if constexpr(STORAGE == mrpt::bayes::particle_storage_mode::POINTER)
201  {
202  // For efficiency, just copy the "CParticleData" from the old particle
203  // into the new one, but this can be done only once:
204  const auto N_old = old_particles.size();
205  std::vector<bool> oldParticleAlreadyCopied(N_old, false);
206  std::vector<PARTICLE_TYPE*> oldParticleFirstCopies(N_old, nullptr);
207 
208  auto newPartIt = newParticlesArray.begin();
209  for (size_t i = 0; newPartIt != newParticlesArray.end();
210  ++newPartIt, ++i)
211  {
212  // The weight:
213  newPartIt->log_w = newParticlesWeight[i];
214 
215  // The data (CParticleData):
216  PARTICLE_TYPE* newPartData;
217  const size_t i_in_old = newParticlesDerivedFromIdx[i];
218  if (!oldParticleAlreadyCopied[i_in_old])
219  {
220  // The first copy of this old particle:
221  newPartData = old_particles[i_in_old].d.release();
222  oldParticleAlreadyCopied[i_in_old] = true;
223  oldParticleFirstCopies[i_in_old] = newPartData;
224  }
225  else
226  {
227  // Make a copy:
228  ASSERT_(oldParticleFirstCopies[i_in_old]);
229  newPartData =
230  new PARTICLE_TYPE(*oldParticleFirstCopies[i_in_old]);
231  }
232 
233  newPartIt->d.reset(newPartData);
234  } // end for "newPartIt"
235 
236  // Now add the new robot pose to the paths:
237  // (this MUST be done after the above loop, separately):
238  // Update the particle with the new pose: this part is caller-dependant
239  // and must be implemented there:
240  newPartIt = newParticlesArray.begin();
241  for (size_t i = 0; i < N;
242  ++newPartIt, ++i)
244  newPartIt->d.get(), newParticles[i]);
245  }
246  else
247  {
248  // Particles as values:
249 
250  auto newPartIt = newParticlesArray.begin();
251  for (size_t i = 0; newPartIt != newParticlesArray.end();
252  ++newPartIt, ++i)
253  {
254  newPartIt->log_w = newParticlesWeight[i];
255  const size_t i_in_old = newParticlesDerivedFromIdx[i];
256  newPartIt->d = old_particles[i_in_old].d;
257  }
258 
259  // Now add the new robot pose to the paths:
260  newPartIt = newParticlesArray.begin();
261  for (size_t i = 0; i < N; ++newPartIt, ++i)
263  &newPartIt->d, newParticles[i]);
264  }
265  // Move to "m_particles"
266  old_particles = std::move(newParticlesArray);
267  } // end of PF_SLAM_implementation_replaceByNewParticleSet
268 
270  const typename mrpt::bayes::CParticleFilterData<
271  PARTICLE_TYPE,STORAGE>::CParticleList& particles,
272  const mrpt::obs::CSensoryFrame* sf) const
273  {
274  MRPT_UNUSED_PARAM(particles);
275  MRPT_UNUSED_PARAM(sf);
276  return true; // By default, always process the SFs.
277  }
278 
279  /** Make a specialization if needed, eg. in the first step in SLAM. */
281  {
282  return false; // By default, always allow the robot to move!
283  }
284 
285  /** Evaluate the observation likelihood for one particle at a given location
286  */
289  const size_t particleIndexForMap,
290  const mrpt::obs::CSensoryFrame& observation,
291  const mrpt::poses::CPose3D& x) const = 0;
292 
293  /** @} */
294 
295  /** Auxiliary method called by PF implementations: return true if we have
296  * both action & observation,
297  * otherwise, return false AND accumulate the odometry so when we have an
298  * observation we didn't lose a thing.
299  * On return=true, the "m_movementDrawer" member is loaded and ready to
300  * draw samples of the increment of pose since last step.
301  * This method is smart enough to accumulate CActionRobotMovement2D or
302  * CActionRobotMovement3D, whatever comes in.
303  */
304  template <class BINTYPE> // Template arg. actually not used, just to allow
305  // giving the definition in another file later on
307  const mrpt::obs::CActionCollection* actions,
308  const mrpt::obs::CSensoryFrame* sf);
309 
310  private:
311  /** The shared implementation body of two PF methods: APF and Optimal-APF,
312  * depending on USE_OPTIMAL_SAMPLING */
313  template <class BINTYPE>
315  const mrpt::obs::CActionCollection* actions,
316  const mrpt::obs::CSensoryFrame* sf,
318  const TKLDParams& KLD_options, const bool USE_OPTIMAL_SAMPLING);
319 
320  template <class BINTYPE>
322  const bool USE_OPTIMAL_SAMPLING, const bool doResample,
323  const double maxMeanLik,
324  size_t k, // The particle from the old set "m_particles[]"
325  const mrpt::obs::CSensoryFrame* sf,
327  mrpt::poses::CPose3D& out_newPose, double& out_newParticleLogWeight);
328 
329 }; // end PF_implementation
330 }
331 }
332 
333 #endif
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...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
Option set for KLD algorithm.
Definition: TKLDParams.h:20
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 bool PF_SLAM_implementation_doWeHaveValidObservations(const typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE, STORAGE >::CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const
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...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
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:113
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)
GLuint index
Definition: glext.h:4054
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:54
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...
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_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.
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:88
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).
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...
GLenum GLint x
Definition: glext.h:3538
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...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



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