Main MRPT website > C++ reference for MRPT 1.5.3
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-2017, 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 #include <mrpt/slam/link_pragmas.h>
23 
24 namespace mrpt
25 {
26  namespace slam
27  {
28  // Frwd decl:
29  template <class PARTICLETYPE, class BINTYPE>
31  BINTYPE &outBin,
32  const TKLDParams &opts,
33  const PARTICLETYPE *currentParticleValue = NULL,
34  const mrpt::math::TPose3D *newPoseToBeInserted = NULL );
35 
36 
37  /** A set of common data shared by PF implementations for both SLAM and localization
38  * \ingroup mrpt_slam_grp
39  */
40  template <class PARTICLE_TYPE, class MYSELF>
42  public mrpt::utils::COutputLogger
43  {
44  public:
46  mrpt::utils::COutputLogger("PF_implementation"),
49  {
50  }
51  protected:
52  /** \name Data members and methods used by generic PF implementations
53  @{ */
54 
59 
60  mrpt::poses::CPoseRandomSampler m_movementDrawer; //!< Used in al PF implementations. \sa PF_SLAM_implementation_gatherActionsCheckBothActObs
61  mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
62  mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb; //!< Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
63  mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
64  mutable std::vector<mrpt::math::TPose3D> m_pfAuxiliaryPFOptimal_maxLikDrawnMovement; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
66 
67  /** Compute w[i]*p(z_t | mu_t^i), with mu_t^i being
68  * the mean of the new robot pose
69  *
70  * \param action MUST be a "const CPose3D*"
71  * \param observation MUST be a "const CSensoryFrame*"
72  */
73  template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
77  size_t index,
78  const void * action,
79  const void * observation );
80 
81  template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
85  size_t index,
86  const void *action,
87  const void *observation );
88 
89  /** @} */
90 
91  /** \name The generic PF implementations for localization & SLAM.
92  @{ */
93 
94  /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation),
95  * common to both localization and mapping.
96  *
97  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
98  *
99  * This method implements optimal sampling with a rejection sampling-based 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 in
104  * Robot Localization," in Proc. IEEE International Conference on Robotics
105  * and Automation (ICRA'08), 2008, pp. 461-466.
106  */
107  template <class BINTYPE>
109  const mrpt::obs::CActionCollection * actions,
110  const mrpt::obs::CSensoryFrame * sf,
112  const TKLDParams &KLD_options);
113 
114  /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal),
115  * common to both localization and mapping.
116  *
117  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
118  *
119  * This method is described in the paper:
120  * Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary Particle Filters".
121  * Journal of the American Statistical Association 94 (446): 590-591. doi:10.2307/2670179.
122  *
123  */
124  template <class BINTYPE>
126  const mrpt::obs::CActionCollection * actions,
127  const mrpt::obs::CSensoryFrame * sf,
129  const TKLDParams &KLD_options);
130 
131 
132  /** A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter),
133  * common to both localization and mapping.
134  *
135  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
136  */
137  template <class BINTYPE>
139  const mrpt::obs::CActionCollection * actions,
140  const mrpt::obs::CSensoryFrame * sf,
142  const TKLDParams &KLD_options);
143 
144  /** @} */
145 
146 
147  public:
148  /** \name Virtual methods that the PF_implementations assume exist.
149  @{ */
150 
151  /** Return the last robot pose in the i'th particle; pose_is_valid will be false if particle is a path and it's empty. */
152  virtual mrpt::math::TPose3D getLastPose(const size_t i, bool &pose_is_valid) const = 0;
153 
155  PARTICLE_TYPE *particleData,
156  const mrpt::math::TPose3D &newPose) const = 0;
157 
158  /** This is the default algorithm to efficiently replace one old set of samples by another new set.
159  * The method uses pointers to make fast copies the first time each particle is duplicated, then
160  * makes real copies for the next ones.
161  *
162  * Note that more efficient specializations might exist for specific particle data structs.
163  */
166  const std::vector<mrpt::math::TPose3D> &newParticles,
167  const std::vector<double> &newParticlesWeight,
168  const std::vector<size_t> &newParticlesDerivedFromIdx ) const
169  {
170  // ---------------------------------------------------------------------------------
171  // Substitute old by new particle set:
172  // Old are in "m_particles"
173  // New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
174  // ---------------------------------------------------------------------------------
175  const size_t N = newParticles.size();
176  typename MYSELF::CParticleList newParticlesArray(N);
177 
178  // For efficiency, just copy the "CParticleData" from the old particle into the
179  // new one, but this can be done only once:
180  std::vector<bool> oldParticleAlreadyCopied(old_particles.size(),false);
181 
182  size_t i;
183  typename MYSELF::CParticleList::iterator newPartIt;
184  for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();++newPartIt,++i)
185  {
186  // The weight:
187  newPartIt->log_w = newParticlesWeight[i];
188 
189  // The data (CParticleData):
190  PARTICLE_TYPE *newPartData;
191  if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
192  {
193  // The first copy of this old particle:
194  newPartData = old_particles[newParticlesDerivedFromIdx[i]].d.release();
195  oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] = true;
196  }
197  else
198  {
199  // Make a copy:
200  newPartData = new PARTICLE_TYPE( *old_particles[ newParticlesDerivedFromIdx[i] ].d );
201  }
202 
203  newPartIt->d.reset(newPartData);
204  } // end for "newPartIt"
205 
206  // Now add the new robot pose to the paths:
207  // (this MUST be done after the above loop, separately):
208  // Update the particle with the new pose: this part is caller-dependant and must be implemented there:
209  for (newPartIt=newParticlesArray.begin(),i=0;i<N;++newPartIt,++i)
210  PF_SLAM_implementation_custom_update_particle_with_new_pose( newPartIt->d.get(), newParticles[i] );
211 
212  // Free those old m_particles not being copied into the new ones: not needed since use of smart ptr.
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.move_from( 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
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...
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.
virtual bool PF_SLAM_implementation_skipRobotMovement() const
Make a specialization if needed, eg.
Declares a class for storing a collection of robot actions.
virtual mrpt::math::TPose3D getLastPose(const size_t i, bool &pose_is_valid) const =0
Return the last robot pose in the i&#39;th particle; pose_is_valid will be false if particle is a path an...
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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).
virtual bool PF_SLAM_implementation_doWeHaveValidObservations(const typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
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.
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...



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Sun Nov 26 00:44:48 UTC 2017