Main MRPT website > C++ reference for MRPT 1.5.7
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
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.
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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.
GLuint index
Definition: glext.h:3891
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
#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
GLenum GLint x
Definition: glext.h:3516
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.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019