Main MRPT website > C++ reference
MRPT logo
PF_implementations_data.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef PF_implementations_data_H
29 #define PF_implementations_data_H
30 
31 #include <mrpt/slam/CActionRobotMovement2D.h>
32 #include <mrpt/bayes/CParticleFilterData.h>
33 #include <mrpt/math/lightweight_geom_data.h>
34 #include <mrpt/poses/CPose3D.h>
35 #include <mrpt/poses/CPose3DPDFGaussian.h>
36 #include <mrpt/poses/CPoseRandomSampler.h>
37 #include <mrpt/slam/TKLDParams.h>
38 
39 #include <mrpt/slam/link_pragmas.h>
40 
41 namespace mrpt
42 {
43  namespace slam
44  {
45  using namespace std;
46  using namespace mrpt::poses;
47  using namespace mrpt::bayes;
48  using namespace mrpt::math;
49 
50 
51  // Frwd decl:
52  template <class PARTICLETYPE, class BINTYPE>
53  void KLF_loadBinFromParticle(
54  BINTYPE &outBin,
55  const TKLDParams &opts,
56  const PARTICLETYPE *currentParticleValue = NULL,
57  const TPose3D *newPoseToBeInserted = NULL );
58 
59 
60  /** A set of common data shared by PF implementations for both SLAM and localization
61  * \ingroup mrpt_slam_grp
62  */
63  template <class PARTICLE_TYPE, class MYSELF>
64  class PF_implementation
65  {
66  public:
67  PF_implementation() :
68  m_accumRobotMovement2DIsValid(false),
69  m_accumRobotMovement3DIsValid(false)
70  {
71  }
72 
73  //typedef mrpt::bayes::CParticleFilterData<PARTICLE_TYPE> BASE;
74  //typedef mrpt::bayes::CParticleFilterCapable BASE2;
75 
76  protected:
77 
78  //BASE &m_partdata; //!< the particle filter data object, implementing CParticleFilterData<PARTICLE_TYPE>
79  //BASE2 &m_pfc; //!< the particle filter data object, implementing CParticleFilterCapable
80 
81  /** \name Data members and methods used by generic PF implementations
82  @{ */
83 
84  CActionRobotMovement2D m_accumRobotMovement2D;
85  bool m_accumRobotMovement2DIsValid;
86  CPose3DPDFGaussian m_accumRobotMovement3D;
87  bool m_accumRobotMovement3DIsValid;
88 
89  CPoseRandomSampler m_movementDrawer; //!< Used in al PF implementations. \sa PF_SLAM_implementation_gatherActionsCheckBothActObs
90  mutable vector_double m_pfAuxiliaryPFOptimal_estimatedProb; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
91  mutable vector_double m_pfAuxiliaryPFStandard_estimatedProb; //!< Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
92  mutable vector_double m_pfAuxiliaryPFOptimal_maxLikelihood; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
93  mutable std::vector<TPose3D> m_pfAuxiliaryPFOptimal_maxLikDrawnMovement; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
94  std::vector<bool> m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed;
95 
96  /** Compute w[i]·p(z_t | mu_t^i), with mu_t^i being
97  * the mean of the new robot pose
98  *
99  * \param action MUST be a "const CPose3D*"
100  * \param observation MUST be a "const CSensoryFrame*"
101  */
102  template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
103  static double PF_SLAM_particlesEvaluator_AuxPFStandard(
104  const CParticleFilter::TParticleFilterOptions &PF_options,
105  const CParticleFilterCapable *obj,
106  size_t index,
107  const void * action,
108  const void * observation );
109 
110  template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
111  static double PF_SLAM_particlesEvaluator_AuxPFOptimal(
112  const CParticleFilter::TParticleFilterOptions &PF_options,
113  const CParticleFilterCapable *obj,
114  size_t index,
115  const void *action,
116  const void *observation );
117 
118  /** @} */
119 
120  /** \name The generic PF implementations for localization & SLAM.
121  @{ */
122 
123  /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation),
124  * common to both localization and mapping.
125  *
126  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
127  *
128  * This method implements optimal sampling with a rejection sampling-based approximation of the true posterior.
129  * For details, see the papers:
130  *
131  * J.-L. Blanco, J. González, and J.-A. Fernández-Madrigal,
132  * "An Optimal Filtering Algorithm for Non-Parametric Observation Models in
133  * Robot Localization," in Proc. IEEE International Conference on Robotics
134  * and Automation (ICRA'08), 2008, pp. 461–466.
135  */
136  template <class BINTYPE>
137  void PF_SLAM_implementation_pfAuxiliaryPFOptimal(
138  const CActionCollection * actions,
139  const CSensoryFrame * sf,
140  const CParticleFilter::TParticleFilterOptions &PF_options,
141  const TKLDParams &KLD_options);
142 
143  /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal),
144  * common to both localization and mapping.
145  *
146  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
147  *
148  * This method is described in the paper:
149  * Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary Particle Filters".
150  * Journal of the American Statistical Association 94 (446): 590–591. doi:10.2307/2670179.
151  *
152  */
153  template <class BINTYPE>
154  void PF_SLAM_implementation_pfAuxiliaryPFStandard(
155  const CActionCollection * actions,
156  const CSensoryFrame * sf,
157  const CParticleFilter::TParticleFilterOptions &PF_options,
158  const TKLDParams &KLD_options);
159 
160 
161  /** A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter),
162  * common to both localization and mapping.
163  *
164  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
165  */
166  template <class BINTYPE>
167  void PF_SLAM_implementation_pfStandardProposal(
168  const CActionCollection * actions,
169  const CSensoryFrame * sf,
170  const CParticleFilter::TParticleFilterOptions &PF_options,
171  const TKLDParams &KLD_options);
172 
173  /** @} */
174 
175 
176  public:
177  /** \name Virtual methods that the PF_implementations assume exist.
178  @{ */
179 
180  /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
181  virtual const TPose3D * getLastPose(const size_t i) const = 0;
182 
183  virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(
184  PARTICLE_TYPE *particleData,
185  const TPose3D &newPose) const = 0;
186 
187  /** This is the default algorithm to efficiently replace one old set of samples by another new set.
188  * The method uses pointers to make fast copies the first time each particle is duplicated, then
189  * makes real copies for the next ones.
190  *
191  * Note that more efficient specializations might exist for specific particle data structs.
192  */
193  virtual void PF_SLAM_implementation_replaceByNewParticleSet(
194  typename CParticleFilterData<PARTICLE_TYPE>::CParticleList &old_particles,
195  const vector<TPose3D> &newParticles,
196  const vector_double &newParticlesWeight,
197  const vector<size_t> &newParticlesDerivedFromIdx ) const
198  {
199  // ---------------------------------------------------------------------------------
200  // Substitute old by new particle set:
201  // Old are in "m_particles"
202  // New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
203  // ---------------------------------------------------------------------------------
204  const size_t N = newParticles.size();
205  typename MYSELF::CParticleList newParticlesArray(N);
206 
207  // For efficiency, just copy the "CParticleData" from the old particle into the
208  // new one, but this can be done only once:
209  std::vector<bool> oldParticleAlreadyCopied(old_particles.size(),false);
210 
211  size_t i;
212  typename MYSELF::CParticleList::iterator newPartIt;
213  for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();newPartIt++,i++)
214  {
215  // The weight:
216  newPartIt->log_w = newParticlesWeight[i];
217 
218  // The data (CParticleData):
219  PARTICLE_TYPE *newPartData;
220  if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
221  {
222  // The first copy of this old particle:
223  newPartData = old_particles[ newParticlesDerivedFromIdx[i] ].d;
224  oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] = true;
225  }
226  else
227  {
228  // Make a copy:
229  newPartData = new PARTICLE_TYPE( *old_particles[ newParticlesDerivedFromIdx[i] ].d );
230  }
231 
232  newPartIt->d = newPartData;
233  } // end for "newPartIt"
234 
235  // Now add the new robot pose to the paths:
236  // (this MUST be done after the above loop, separately):
237  // Update the particle with the new pose: this part is caller-dependant and must be implemented there:
238  for (newPartIt=newParticlesArray.begin(),i=0;i<N;++newPartIt,++i)
239  PF_SLAM_implementation_custom_update_particle_with_new_pose( newPartIt->d, newParticles[i] );
240 
241  // Free those old m_particles not being copied into the new ones:
242  for (size_t i=0;i<old_particles.size();i++)
243  if (!oldParticleAlreadyCopied[i])
244  mrpt::utils::delete_safe( old_particles[ i ].d );
245 
246  // Copy into "m_particles"
247  old_particles.resize( newParticlesArray.size() );
248  typename MYSELF::CParticleList::iterator trgPartIt;
249  for (newPartIt=newParticlesArray.begin(),trgPartIt=old_particles.begin(); newPartIt!=newParticlesArray.end(); ++newPartIt, ++trgPartIt )
250  {
251  trgPartIt->log_w = newPartIt->log_w;
252  trgPartIt->d = newPartIt->d;
253  }
254  } // end of PF_SLAM_implementation_replaceByNewParticleSet
255 
256 
257 
258  virtual bool PF_SLAM_implementation_doWeHaveValidObservations(
259  const typename CParticleFilterData<PARTICLE_TYPE>::CParticleList &particles,
260  const CSensoryFrame *sf) const
261  {
262  return true; // By default, always process the SFs.
263  }
264 
265  /** Make a specialization if needed, eg. in the first step in SLAM. */
266  virtual bool PF_SLAM_implementation_skipRobotMovement() const
267  {
268  return false; // By default, always allow the robot to move!
269  }
270 
271  /** Evaluate the observation likelihood for one particle at a given location */
272  virtual double PF_SLAM_computeObservationLikelihoodForParticle(
273  const CParticleFilter::TParticleFilterOptions &PF_options,
274  const size_t particleIndexForMap,
275  const CSensoryFrame &observation,
276  const CPose3D &x ) const = 0;
277 
278  /** @} */
279 
280 
281  /** Auxiliary method called by PF implementations: return true if we have both action & observation,
282  * otherwise, return false AND accumulate the odometry so when we have an observation we didn't lose a thing.
283  * On return=true, the "m_movementDrawer" member is loaded and ready to draw samples of the increment of pose since last step.
284  * This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.
285  */
286  template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
287  bool PF_SLAM_implementation_gatherActionsCheckBothActObs(
288  const CActionCollection * actions,
289  const CSensoryFrame * sf );
290 
291  private:
292  /** The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPLING */
293  template <class BINTYPE>
294  void PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(
295  const CActionCollection * actions,
296  const CSensoryFrame * sf,
297  const CParticleFilter::TParticleFilterOptions &PF_options,
298  const TKLDParams &KLD_options,
299  const bool USE_OPTIMAL_SAMPLING );
300 
301  template <class BINTYPE>
302  void PF_SLAM_aux_perform_one_rejection_sampling_step(
303  const bool USE_OPTIMAL_SAMPLING,
304  const bool doResample,
305  const double maxMeanLik,
306  size_t k, // The particle from the old set "m_particles[]"
307  const CSensoryFrame * sf,
308  const CParticleFilter::TParticleFilterOptions &PF_options,
309  CPose3D & out_newPose,
310  double & out_newParticleLogWeight);
311 
312 
313  }; // end PF_implementation
314  }
315 }
316 
317 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013