Main MRPT website > C++ reference
MRPT logo

PF_implementations_data.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef PF_implementations_data_H
00029 #define PF_implementations_data_H
00030 
00031 #include <mrpt/slam/CActionRobotMovement2D.h>
00032 #include <mrpt/bayes/CParticleFilterData.h>
00033 #include <mrpt/math/lightweight_geom_data.h>
00034 #include <mrpt/poses/CPose3D.h>
00035 #include <mrpt/poses/CPose3DPDFGaussian.h>
00036 #include <mrpt/poses/CPoseRandomSampler.h>
00037 #include <mrpt/slam/TKLDParams.h>
00038 
00039 #include <mrpt/slam/link_pragmas.h>
00040 
00041 namespace mrpt
00042 {
00043         namespace slam
00044         {
00045                 using namespace std;
00046                 using namespace mrpt::poses;
00047                 using namespace mrpt::bayes;
00048                 using namespace mrpt::math;
00049 
00050 
00051                 // Frwd decl:
00052                 template <class PARTICLETYPE, class BINTYPE>
00053                 void KLF_loadBinFromParticle(
00054                         BINTYPE                         &outBin,
00055                         const TKLDParams        &opts,
00056                         const PARTICLETYPE      *currentParticleValue = NULL,
00057                         const TPose3D           *newPoseToBeInserted = NULL );
00058 
00059 
00060                 /** A set of common data shared by PF implementations for both SLAM and localization
00061                   */
00062                 template <class PARTICLE_TYPE, class MYSELF>
00063                 class PF_implementation
00064                 {
00065                 public:
00066                         PF_implementation() :
00067                                 m_accumRobotMovement2DIsValid(false),
00068                                 m_accumRobotMovement3DIsValid(false)
00069                         {
00070                         }
00071 
00072                         //typedef mrpt::bayes::CParticleFilterData<PARTICLE_TYPE> BASE;
00073                         //typedef mrpt::bayes::CParticleFilterCapable                           BASE2;
00074 
00075                 protected:
00076 
00077                         //BASE  &m_partdata;  //!< the particle filter data object, implementing CParticleFilterData<PARTICLE_TYPE>
00078                         //BASE2 &m_pfc;           //!< the particle filter data object, implementing CParticleFilterCapable
00079 
00080                         /** \name Data members and methods used by generic PF implementations
00081                             @{ */
00082 
00083                         CActionRobotMovement2D  m_accumRobotMovement2D;
00084                         bool                                    m_accumRobotMovement2DIsValid;
00085                         CPose3DPDFGaussian              m_accumRobotMovement3D;
00086                         bool                                    m_accumRobotMovement3DIsValid;
00087 
00088                         CPoseRandomSampler                              m_movementDrawer;                                               //!< Used in al PF implementations. \sa PF_SLAM_implementation_gatherActionsCheckBothActObs
00089                         mutable vector_double                   m_pfAuxiliaryPFOptimal_estimatedProb;   //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
00090                         mutable vector_double                   m_pfAuxiliaryPFStandard_estimatedProb;  //!< Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
00091                         mutable vector_double                   m_pfAuxiliaryPFOptimal_maxLikelihood;                                           //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
00092                         mutable std::vector<TPose3D>    m_pfAuxiliaryPFOptimal_maxLikDrawnMovement;             //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
00093                         std::vector<bool>                               m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed;
00094 
00095                         /**  Compute w[i]·p(z_t | mu_t^i), with mu_t^i being
00096                           *    the mean of the new robot pose
00097                           *
00098                           * \param action MUST be a "const CPose3D*"
00099                           * \param observation MUST be a "const CSensoryFrame*"
00100                           */
00101                         template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
00102                         static double PF_SLAM_particlesEvaluator_AuxPFStandard(
00103                                 const CParticleFilter::TParticleFilterOptions &PF_options,
00104                                 const CParticleFilterCapable    *obj,
00105                                 size_t index,
00106                                 const void * action,
00107                                 const void * observation );
00108 
00109                         template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
00110                         static double  PF_SLAM_particlesEvaluator_AuxPFOptimal(
00111                                 const CParticleFilter::TParticleFilterOptions &PF_options,
00112                                 const CParticleFilterCapable    *obj,
00113                                 size_t                                  index,
00114                                 const void                              *action,
00115                                 const void                              *observation );
00116 
00117                         /** @} */
00118 
00119                         /** \name The generic PF implementations for localization & SLAM.
00120                             @{ */
00121 
00122                         /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation),
00123                           *  common to both localization and mapping.
00124                           *
00125                           * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
00126                           *
00127                           *  This method implements optimal sampling with a rejection sampling-based approximation of the true posterior.
00128                           *  For details, see the papers:
00129                           *
00130                           *  J.-L. Blanco, J. González, and J.-A. Fernández-Madrigal,
00131                           *    "An Optimal Filtering Algorithm for Non-Parametric Observation Models in
00132                           *     Robot Localization," in Proc. IEEE International Conference on Robotics
00133                           *     and Automation (ICRA'08), 2008, pp. 461–466.
00134                           */
00135                         template <class BINTYPE>
00136                         void PF_SLAM_implementation_pfAuxiliaryPFOptimal(
00137                                 const CActionCollection * actions,
00138                                 const CSensoryFrame             * sf,
00139                                 const CParticleFilter::TParticleFilterOptions &PF_options,
00140                                 const TKLDParams &KLD_options);
00141 
00142                         /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal),
00143                           *  common to both localization and mapping.
00144                           *
00145                           * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
00146                           *
00147                           *  This method is described in the paper:
00148                           *   Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary Particle Filters".
00149                           *    Journal of the American Statistical Association 94 (446): 590–591. doi:10.2307/2670179.
00150                           *
00151                           */
00152                         template <class BINTYPE>
00153                         void PF_SLAM_implementation_pfAuxiliaryPFStandard(
00154                                 const CActionCollection * actions,
00155                                 const CSensoryFrame             * sf,
00156                                 const CParticleFilter::TParticleFilterOptions &PF_options,
00157                                 const TKLDParams &KLD_options);
00158 
00159 
00160                         /** A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter),
00161                           *  common to both localization and mapping.
00162                           *
00163                           * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
00164                           */
00165                         template <class BINTYPE>
00166                         void PF_SLAM_implementation_pfStandardProposal(
00167                                 const CActionCollection * actions,
00168                                 const CSensoryFrame             * sf,
00169                                 const CParticleFilter::TParticleFilterOptions &PF_options,
00170                                 const TKLDParams &KLD_options);
00171 
00172                         /** @} */
00173 
00174 
00175                 public:
00176                         /** \name Virtual methods that the PF_implementations assume exist.
00177                             @{ */
00178 
00179                         /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
00180                         virtual const TPose3D * getLastPose(const size_t i) const = 0;
00181 
00182                         virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(
00183                                 PARTICLE_TYPE *particleData,
00184                                 const TPose3D &newPose) const = 0;
00185 
00186                         /** This is the default algorithm to efficiently replace one old set of samples by another new set.
00187                           *  The method uses pointers to make fast copies the first time each particle is duplicated, then
00188                           *   makes real copies for the next ones.
00189                           *
00190                           *  Note that more efficient specializations might exist for specific particle data structs.
00191                           */
00192                         virtual void PF_SLAM_implementation_replaceByNewParticleSet(
00193                                 typename CParticleFilterData<PARTICLE_TYPE>::CParticleList       &old_particles,
00194                                 const vector<TPose3D>           &newParticles,
00195                                 const vector_double                     &newParticlesWeight,
00196                                 const vector<size_t>            &newParticlesDerivedFromIdx ) const
00197                         {
00198                                 // ---------------------------------------------------------------------------------
00199                                 // Substitute old by new particle set:
00200                                 //   Old are in "m_particles"
00201                                 //   New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
00202                                 // ---------------------------------------------------------------------------------
00203                                 const size_t N = newParticles.size();
00204                                 typename MYSELF::CParticleList newParticlesArray(N);
00205 
00206                                 // For efficiency, just copy the "CParticleData" from the old particle into the
00207                                 //  new one, but this can be done only once:
00208                                 std::vector<bool>       oldParticleAlreadyCopied(old_particles.size(),false);
00209 
00210                                 size_t i;
00211                                 typename MYSELF::CParticleList::iterator        newPartIt;
00212                                 for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();newPartIt++,i++)
00213                                 {
00214                                         // The weight:
00215                                         newPartIt->log_w = newParticlesWeight[i];
00216 
00217                                         // The data (CParticleData):
00218                                         PARTICLE_TYPE *newPartData;
00219                                         if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
00220                                         {
00221                                                 // The first copy of this old particle:
00222                                                 newPartData = old_particles[ newParticlesDerivedFromIdx[i] ].d;
00223                                                 oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] = true;
00224                                         }
00225                                         else
00226                                         {
00227                                                 // Make a copy:
00228                                                 newPartData = new PARTICLE_TYPE( *old_particles[ newParticlesDerivedFromIdx[i] ].d );
00229                                         }
00230 
00231                                         newPartIt->d = newPartData;
00232                                 } // end for "newPartIt"
00233 
00234                                 // Now add the new robot pose to the paths:
00235                                 //  (this MUST be done after the above loop, separately):
00236                                 // Update the particle with the new pose: this part is caller-dependant and must be implemented there:
00237                                 for (newPartIt=newParticlesArray.begin(),i=0;i<N;++newPartIt,++i)
00238                                         PF_SLAM_implementation_custom_update_particle_with_new_pose( newPartIt->d, newParticles[i] );
00239 
00240                                 // Free those old m_particles not being copied into the new ones:
00241                                 for (size_t i=0;i<old_particles.size();i++)
00242                                         if (!oldParticleAlreadyCopied[i])
00243                                                 mrpt::utils::delete_safe( old_particles[ i ].d );
00244 
00245                                 // Copy into "m_particles"
00246                                 old_particles.resize( newParticlesArray.size() );
00247                                 typename MYSELF::CParticleList::iterator        trgPartIt;
00248                                 for (newPartIt=newParticlesArray.begin(),trgPartIt=old_particles.begin(); newPartIt!=newParticlesArray.end(); ++newPartIt, ++trgPartIt )
00249                                 {
00250                                         trgPartIt->log_w = newPartIt->log_w;
00251                                         trgPartIt->d = newPartIt->d;
00252                                 }
00253                         } // end of PF_SLAM_implementation_replaceByNewParticleSet
00254 
00255 
00256 
00257                         virtual bool PF_SLAM_implementation_doWeHaveValidObservations(
00258                                 const typename CParticleFilterData<PARTICLE_TYPE>::CParticleList        &particles,
00259                                 const CSensoryFrame *sf) const
00260                         {
00261                                 return true;    // By default, always process the SFs.
00262                         }
00263 
00264                         /** Make a specialization if needed, eg. in the first step in SLAM.  */
00265                         virtual bool PF_SLAM_implementation_skipRobotMovement() const
00266                         {
00267                                 return false; // By default, always allow the robot to move!
00268                         }
00269 
00270                         /** Evaluate the observation likelihood for one particle at a given location */
00271                         virtual double PF_SLAM_computeObservationLikelihoodForParticle(
00272                                 const CParticleFilter::TParticleFilterOptions   &PF_options,
00273                                 const size_t                    particleIndexForMap,
00274                                 const CSensoryFrame             &observation,
00275                                 const CPose3D                   &x )  const = 0;
00276 
00277                         /** @} */
00278 
00279 
00280                 /** Auxiliary method called by PF implementations: return true if we have both action & observation,
00281                   *   otherwise, return false AND accumulate the odometry so when we have an observation we didn't lose a thing.
00282                   *   On return=true, the "m_movementDrawer" member is loaded and ready to draw samples of the increment of pose since last step.
00283                   *  This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.
00284                   */
00285                 template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
00286                 bool PF_SLAM_implementation_gatherActionsCheckBothActObs(
00287                         const CActionCollection * actions,
00288                         const CSensoryFrame             * sf );
00289 
00290                 private:
00291                         /** The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPLING */
00292                         template <class BINTYPE>
00293                         void PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(
00294                                 const CActionCollection * actions,
00295                                 const CSensoryFrame             * sf,
00296                                 const CParticleFilter::TParticleFilterOptions &PF_options,
00297                                 const TKLDParams &KLD_options,
00298                                 const bool USE_OPTIMAL_SAMPLING  );
00299 
00300                         template <class BINTYPE>
00301                         void PF_SLAM_aux_perform_one_rejection_sampling_step(
00302                                 const bool              USE_OPTIMAL_SAMPLING,
00303                                 const bool              doResample,
00304                                 const double    maxMeanLik,
00305                                 size_t    k, // The particle from the old set "m_particles[]"
00306                                 const CSensoryFrame             * sf,
00307                                 const CParticleFilter::TParticleFilterOptions &PF_options,
00308                                 CPose3D                 & out_newPose,
00309                                 double                  & out_newParticleLogWeight);
00310 
00311 
00312                 }; // end PF_implementation
00313         }
00314 }
00315 
00316 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011