Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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>
20 
21 #include <mrpt/slam/link_pragmas.h>
22 
23 namespace mrpt
24 {
25  namespace slam
26  {
27  // Frwd decl:
28  template <class PARTICLETYPE, class BINTYPE>
30  BINTYPE &outBin,
31  const TKLDParams &opts,
32  const PARTICLETYPE *currentParticleValue = NULL,
33  const mrpt::math::TPose3D *newPoseToBeInserted = NULL );
34 
35 
36  /** A set of common data shared by PF implementations for both SLAM and localization
37  * \ingroup mrpt_slam_grp
38  */
39  template <class PARTICLE_TYPE, class MYSELF>
41  {
42  public:
46  {
47  }
48  protected:
49  /** \name Data members and methods used by generic PF implementations
50  @{ */
51 
56 
57  mrpt::poses::CPoseRandomSampler m_movementDrawer; //!< Used in al PF implementations. \sa PF_SLAM_implementation_gatherActionsCheckBothActObs
58  mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
59  mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb; //!< Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
60  mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
61  mutable std::vector<mrpt::math::TPose3D> m_pfAuxiliaryPFOptimal_maxLikDrawnMovement; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
63 
64  /** Compute w[i]*p(z_t | mu_t^i), with mu_t^i being
65  * the mean of the new robot pose
66  *
67  * \param action MUST be a "const CPose3D*"
68  * \param observation MUST be a "const CSensoryFrame*"
69  */
70  template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
74  size_t index,
75  const void * action,
76  const void * observation );
77 
78  template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
82  size_t index,
83  const void *action,
84  const void *observation );
85 
86  /** @} */
87 
88  /** \name The generic PF implementations for localization & SLAM.
89  @{ */
90 
91  /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation),
92  * common to both localization and mapping.
93  *
94  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
95  *
96  * This method implements optimal sampling with a rejection sampling-based approximation of the true posterior.
97  * For details, see the papers:
98  *
99  * J.L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal,
100  * "An Optimal Filtering Algorithm for Non-Parametric Observation Models in
101  * Robot Localization," in Proc. IEEE International Conference on Robotics
102  * and Automation (ICRA'08), 2008, pp. 461-466.
103  */
104  template <class BINTYPE>
106  const mrpt::obs::CActionCollection * actions,
107  const mrpt::obs::CSensoryFrame * sf,
109  const TKLDParams &KLD_options);
110 
111  /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal),
112  * common to both localization and mapping.
113  *
114  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
115  *
116  * This method is described in the paper:
117  * Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary Particle Filters".
118  * Journal of the American Statistical Association 94 (446): 590-591. doi:10.2307/2670179.
119  *
120  */
121  template <class BINTYPE>
123  const mrpt::obs::CActionCollection * actions,
124  const mrpt::obs::CSensoryFrame * sf,
126  const TKLDParams &KLD_options);
127 
128 
129  /** A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter),
130  * common to both localization and mapping.
131  *
132  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
133  */
134  template <class BINTYPE>
136  const mrpt::obs::CActionCollection * actions,
137  const mrpt::obs::CSensoryFrame * sf,
139  const TKLDParams &KLD_options);
140 
141  /** @} */
142 
143 
144  public:
145  /** \name Virtual methods that the PF_implementations assume exist.
146  @{ */
147 
148  /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
149  virtual const mrpt::math::TPose3D * getLastPose(const size_t i) const = 0;
150 
152  PARTICLE_TYPE *particleData,
153  const mrpt::math::TPose3D &newPose) const = 0;
154 
155  /** This is the default algorithm to efficiently replace one old set of samples by another new set.
156  * The method uses pointers to make fast copies the first time each particle is duplicated, then
157  * makes real copies for the next ones.
158  *
159  * Note that more efficient specializations might exist for specific particle data structs.
160  */
163  const std::vector<mrpt::math::TPose3D> &newParticles,
164  const std::vector<double> &newParticlesWeight,
165  const std::vector<size_t> &newParticlesDerivedFromIdx ) const
166  {
167  // ---------------------------------------------------------------------------------
168  // Substitute old by new particle set:
169  // Old are in "m_particles"
170  // New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
171  // ---------------------------------------------------------------------------------
172  const size_t N = newParticles.size();
173  typename MYSELF::CParticleList newParticlesArray(N);
174 
175  // For efficiency, just copy the "CParticleData" from the old particle into the
176  // new one, but this can be done only once:
177  std::vector<bool> oldParticleAlreadyCopied(old_particles.size(),false);
178 
179  size_t i;
180  typename MYSELF::CParticleList::iterator newPartIt;
181  for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();++newPartIt,++i)
182  {
183  // The weight:
184  newPartIt->log_w = newParticlesWeight[i];
185 
186  // The data (CParticleData):
187  PARTICLE_TYPE *newPartData;
188  if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
189  {
190  // The first copy of this old particle:
191  newPartData = old_particles[ newParticlesDerivedFromIdx[i] ].d;
192  oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] = true;
193  }
194  else
195  {
196  // Make a copy:
197  newPartData = new PARTICLE_TYPE( *old_particles[ newParticlesDerivedFromIdx[i] ].d );
198  }
199 
200  newPartIt->d = newPartData;
201  } // end for "newPartIt"
202 
203  // Now add the new robot pose to the paths:
204  // (this MUST be done after the above loop, separately):
205  // Update the particle with the new pose: this part is caller-dependant and must be implemented there:
206  for (newPartIt=newParticlesArray.begin(),i=0;i<N;++newPartIt,++i)
207  PF_SLAM_implementation_custom_update_particle_with_new_pose( newPartIt->d, newParticles[i] );
208 
209  // Free those old m_particles not being copied into the new ones:
210  for (size_t i=0;i<old_particles.size();i++)
211  if (!oldParticleAlreadyCopied[i])
212  mrpt::utils::delete_safe( old_particles[ i ].d );
213 
214  // Copy into "m_particles"
215  old_particles.resize( newParticlesArray.size() );
216  typename MYSELF::CParticleList::iterator trgPartIt;
217  for (newPartIt=newParticlesArray.begin(),trgPartIt=old_particles.begin(); newPartIt!=newParticlesArray.end(); ++newPartIt, ++trgPartIt )
218  {
219  trgPartIt->log_w = newPartIt->log_w;
220  trgPartIt->d = newPartIt->d;
221  }
222  } // end of PF_SLAM_implementation_replaceByNewParticleSet
223 
224 
225 
228  const mrpt::obs::CSensoryFrame *sf) const
229  {
230  MRPT_UNUSED_PARAM(particles); MRPT_UNUSED_PARAM(sf);
231  return true; // By default, always process the SFs.
232  }
233 
234  /** Make a specialization if needed, eg. in the first step in SLAM. */
236  {
237  return false; // By default, always allow the robot to move!
238  }
239 
240  /** Evaluate the observation likelihood for one particle at a given location */
243  const size_t particleIndexForMap,
244  const mrpt::obs::CSensoryFrame &observation,
245  const mrpt::poses::CPose3D &x ) const = 0;
246 
247  /** @} */
248 
249 
250  /** Auxiliary method called by PF implementations: return true if we have both action & observation,
251  * otherwise, return false AND accumulate the odometry so when we have an observation we didn't lose a thing.
252  * On return=true, the "m_movementDrawer" member is loaded and ready to draw samples of the increment of pose since last step.
253  * This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.
254  */
255  template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
257  const mrpt::obs::CActionCollection * actions,
258  const mrpt::obs::CSensoryFrame * sf );
259 
260  private:
261  /** The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPLING */
262  template <class BINTYPE>
264  const mrpt::obs::CActionCollection * actions,
265  const mrpt::obs::CSensoryFrame * sf,
267  const TKLDParams &KLD_options,
268  const bool USE_OPTIMAL_SAMPLING );
269 
270  template <class BINTYPE>
272  const bool USE_OPTIMAL_SAMPLING,
273  const bool doResample,
274  const double maxMeanLik,
275  size_t k, // The particle from the old set "m_particles[]"
276  const mrpt::obs::CSensoryFrame * sf,
278  mrpt::poses::CPose3D & out_newPose,
279  double & out_newParticleLogWeight);
280 
281 
282  }; // end PF_implementation
283  }
284 }
285 
286 #endif
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...
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement2D
virtual bool PF_SLAM_implementation_doWeHaveValidObservations(const typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const
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)
mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
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...
virtual void PF_SLAM_implementation_replaceByNewParticleSet(typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::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...
Scalar * iterator
Definition: eigen_plugins.h:23
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
Option set for KLD algorithm.
Definition: TKLDParams.h:22
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...
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::CPoseRandomSampler m_movementDrawer
Used in al PF implementations.
Declares a class for storing a collection of robot actions.
A set of common data shared by PF implementations for both SLAM and localization. ...
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)
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...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
This virtual class defines the interface that any particles based PDF class must implement in order t...
virtual const mrpt::math::TPose3D * getLastPose(const size_t i) const =0
Return a pointer to the last robot pose in the i&#39;th particle (or NULL if it&#39;s a path and it&#39;s empty)...
virtual bool PF_SLAM_implementation_skipRobotMovement() const
Make a specialization if needed, eg.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
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.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void KLF_loadBinFromParticle(BINTYPE &outBin, const TKLDParams &opts, const PARTICLETYPE *currentParticleValue=NULL, const mrpt::math::TPose3D *newPoseToBeInserted=NULL)
mrpt::poses::CPose3DPDFGaussian m_accumRobotMovement3D
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
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...
std::vector< bool > m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed
virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(PARTICLE_TYPE *particleData, const mrpt::math::TPose3D &newPose) const =0
std::vector< mrpt::math::TPose3D > m_pfAuxiliaryPFOptimal_maxLikDrawnMovement
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN:Unversioned directory at Mon Jul 4 10:31:07 UTC 2016