Main MRPT website > C++ reference for MRPT 1.5.6
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(), N_old = old_particles.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(N_old,false);
181  std::vector<PARTICLE_TYPE *> oldParticleFirstCopies(N_old, NULL);
182 
183  size_t i;
184  typename MYSELF::CParticleList::iterator newPartIt;
185  for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();++newPartIt,++i)
186  {
187  // The weight:
188  newPartIt->log_w = newParticlesWeight[i];
189 
190  // The data (CParticleData):
191  PARTICLE_TYPE *newPartData;
192  const size_t i_in_old = newParticlesDerivedFromIdx[i];
193  if (!oldParticleAlreadyCopied[i_in_old])
194  {
195  // The first copy of this old particle:
196  newPartData = old_particles[i_in_old].d.release();
197  oldParticleAlreadyCopied[i_in_old] = true;
198  oldParticleFirstCopies[i_in_old] = newPartData;
199  }
200  else
201  {
202  // Make a copy:
203  ASSERT_(oldParticleFirstCopies[i_in_old]);
204  newPartData = new PARTICLE_TYPE(*oldParticleFirstCopies[i_in_old]);
205  }
206 
207  newPartIt->d.reset(newPartData);
208  } // end for "newPartIt"
209 
210  // Now add the new robot pose to the paths:
211  // (this MUST be done after the above loop, separately):
212  // Update the particle with the new pose: this part is caller-dependant and must be implemented there:
213  for (newPartIt=newParticlesArray.begin(),i=0;i<N;++newPartIt,++i)
214  PF_SLAM_implementation_custom_update_particle_with_new_pose( newPartIt->d.get(), newParticles[i] );
215 
216  // Free those old m_particles not being copied into the new ones: not needed since use of smart ptr.
217 
218  // Copy into "m_particles"
219  old_particles.resize( newParticlesArray.size() );
220  typename MYSELF::CParticleList::iterator trgPartIt;
221  for (newPartIt=newParticlesArray.begin(),trgPartIt=old_particles.begin(); newPartIt!=newParticlesArray.end(); ++newPartIt, ++trgPartIt )
222  {
223  trgPartIt->log_w = newPartIt->log_w;
224  trgPartIt->d.move_from( newPartIt->d );
225  }
226  } // end of PF_SLAM_implementation_replaceByNewParticleSet
227 
228 
229 
232  const mrpt::obs::CSensoryFrame *sf) const
233  {
234  MRPT_UNUSED_PARAM(particles); MRPT_UNUSED_PARAM(sf);
235  return true; // By default, always process the SFs.
236  }
237 
238  /** Make a specialization if needed, eg. in the first step in SLAM. */
240  {
241  return false; // By default, always allow the robot to move!
242  }
243 
244  /** Evaluate the observation likelihood for one particle at a given location */
247  const size_t particleIndexForMap,
248  const mrpt::obs::CSensoryFrame &observation,
249  const mrpt::poses::CPose3D &x ) const = 0;
250 
251  /** @} */
252 
253 
254  /** Auxiliary method called by PF implementations: return true if we have both action & observation,
255  * otherwise, return false AND accumulate the odometry so when we have an observation we didn't lose a thing.
256  * On return=true, the "m_movementDrawer" member is loaded and ready to draw samples of the increment of pose since last step.
257  * This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.
258  */
259  template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
261  const mrpt::obs::CActionCollection * actions,
262  const mrpt::obs::CSensoryFrame * sf );
263 
264  private:
265  /** The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPLING */
266  template <class BINTYPE>
268  const mrpt::obs::CActionCollection * actions,
269  const mrpt::obs::CSensoryFrame * sf,
271  const TKLDParams &KLD_options,
272  const bool USE_OPTIMAL_SAMPLING );
273 
274  template <class BINTYPE>
276  const bool USE_OPTIMAL_SAMPLING,
277  const bool doResample,
278  const double maxMeanLik,
279  size_t k, // The particle from the old set "m_particles[]"
280  const mrpt::obs::CSensoryFrame * sf,
282  mrpt::poses::CPose3D & out_newPose,
283  double & out_newParticleLogWeight);
284 
285 
286  }; // end PF_implementation
287  }
288 }
289 
290 #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.
virtual mrpt::math::TPose3D getLastPose(const size_t i, bool &pose_is_valid) const =0
Return the last robot pose in the i'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.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
GLhandleARB obj
Definition: glew.h:3276
This virtual class defines the interface that any particles based PDF class must implement in order t...
virtual bool PF_SLAM_implementation_skipRobotMovement() const
Make a specialization if needed, eg.
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
GLuint index
Definition: glew.h:1721
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).
#define ASSERT_(f)
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018