Main MRPT website > C++ reference
MRPT logo

CMonteCarloLocalization2D.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 CMonteCarloLocalization2D_H
00029 #define CMonteCarloLocalization2D_H
00030 
00031 #include <mrpt/poses/CPosePDFParticles.h>
00032 #include <mrpt/slam/PF_implementations_data.h>
00033 #include <mrpt/slam/TMonteCarloLocalizationParams.h>
00034 
00035 #include <mrpt/slam/link_pragmas.h>
00036 
00037 namespace mrpt
00038 {
00039         namespace slam
00040         {
00041                 class COccupancyGridMap2D;
00042                 class CSensoryFrame;
00043 
00044                 using namespace mrpt::poses;
00045                 using namespace mrpt::slam;
00046                 using namespace mrpt::bayes;
00047 
00048                 /** Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.
00049                  *
00050                  *  This class also implements particle filtering for robot localization. See the MRPT
00051                  *   application "app/pf-localization" for an example of usage.
00052                  *
00053                  * \sa CMonteCarloLocalization3D, CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable
00054                  */
00055                 class SLAM_IMPEXP CMonteCarloLocalization2D :
00056                         public CPosePDFParticles,
00057                         public PF_implementation<CPose2D,CMonteCarloLocalization2D>
00058                 {
00059                         //template <class PARTICLE_TYPE, class MYSELF> friend class PF_implementation;
00060 
00061                 public:
00062                         TMonteCarloLocalizationParams   options; //!< MCL parameters
00063 
00064                         /** Constructor
00065                           * \param M The number of m_particles.
00066                           */
00067                         CMonteCarloLocalization2D( size_t M = 1 );
00068 
00069                         /** Destructor */
00070                         virtual ~CMonteCarloLocalization2D();
00071 
00072                         /** Reset the PDF to an uniformly distributed one, but only in the free-space
00073                           *   of a given 2D occupancy-grid-map. Orientation is randomly generated in the whole 2*PI range.
00074                           * \param theMap The occupancy grid map
00075                           * \param freeCellsThreshold The minimum free-probability to consider a cell as empty (default is 0.7)
00076                           * \param particlesCount If set to -1 the number of m_particles remains unchanged.
00077                           * \param x_min The limits of the area to look for free cells.
00078                           * \param x_max The limits of the area to look for free cells.
00079                           * \param y_min The limits of the area to look for free cells.
00080                           * \param y_max The limits of the area to look for free cells.
00081                           * \param phi_min The limits of the area to look for free cells.
00082                           * \param phi_max The limits of the area to look for free cells.
00083                           *  \sa resetDeterm32inistic
00084                           * \exception std::exception On any error (no free cell found in map, map=NULL, etc...)
00085                           */
00086                         void  resetUniformFreeSpace(
00087                                                 COccupancyGridMap2D             *theMap,
00088                                                 const double                                    freeCellsThreshold = 0.7,
00089                                                 const int                                               particlesCount = -1,
00090                                                 const double                                    x_min = -1e10f,
00091                                                 const double                                    x_max = 1e10f,
00092                                                 const double                                    y_min = -1e10f,
00093                                                 const double                                    y_max = 1e10f,
00094                                                 const double                                    phi_min = -M_PI,
00095                                                 const double                                    phi_max = M_PI );
00096 
00097                          /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
00098                           *  This method has additional configuration parameters in "options".
00099                           *  Performs the update stage of the RBPF, using the sensed CSensoryFrame:
00100                           *
00101                           *   \param action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
00102                           *   \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
00103                           *
00104                           * \sa options
00105                           */
00106                         void  prediction_and_update_pfStandardProposal(
00107                                 const mrpt::slam::CActionCollection     * action,
00108                                 const mrpt::slam::CSensoryFrame         * observation,
00109                                 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00110 
00111                          /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
00112                           *  This method has additional configuration parameters in "options".
00113                           *  Performs the update stage of the RBPF, using the sensed CSensoryFrame:
00114                           *
00115                           *   \param Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
00116                           *   \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
00117                           *
00118                           * \sa options
00119                           */
00120                         void  prediction_and_update_pfAuxiliaryPFStandard(
00121                                 const mrpt::slam::CActionCollection     * action,
00122                                 const mrpt::slam::CSensoryFrame         * observation,
00123                                 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00124 
00125                          /** Update the m_particles, predicting the posterior of robot pose and map after a movement command.
00126                           *  This method has additional configuration parameters in "options".
00127                           *  Performs the update stage of the RBPF, using the sensed CSensoryFrame:
00128                           *
00129                           *   \param Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
00130                           *   \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
00131                           *
00132                           * \sa options
00133                           */
00134                         void  prediction_and_update_pfAuxiliaryPFOptimal(
00135                                 const mrpt::slam::CActionCollection     * action,
00136                                 const mrpt::slam::CSensoryFrame         * observation,
00137                                 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00138 
00139                 //protected:
00140                         /** \name Virtual methods that the PF_implementations assume exist.
00141                             @{ */
00142                         /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
00143                         const TPose3D * getLastPose(const size_t i) const;
00144 
00145                         void PF_SLAM_implementation_custom_update_particle_with_new_pose(
00146                                 CParticleDataContent *particleData,
00147                                 const TPose3D &newPose) const;
00148 
00149                         // We'll redefine this one:
00150                         void PF_SLAM_implementation_replaceByNewParticleSet(
00151                                 CParticleList   &old_particles,
00152                                 const std::vector<TPose3D>      &newParticles,
00153                                 const vector_double             &newParticlesWeight,
00154                                 const std::vector<size_t>       &newParticlesDerivedFromIdx )  const;
00155 
00156                         /** Evaluate the observation likelihood for one particle at a given location */
00157                         double PF_SLAM_computeObservationLikelihoodForParticle(
00158                                 const CParticleFilter::TParticleFilterOptions   &PF_options,
00159                                 const size_t                    particleIndexForMap,
00160                                 const CSensoryFrame             &observation,
00161                                 const CPose3D                   &x ) const;
00162                         /** @} */
00163 
00164 
00165                 }; // End of class def.
00166 
00167         } // End of namespace
00168 } // End of namespace
00169 
00170 #endif



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