Main MRPT website > C++ reference
MRPT logo

CMetricMapBuilderRBPF.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 CMetricMapBuilderRBPF_H
00029 #define CMetricMapBuilderRBPF_H
00030 
00031 #include <mrpt/slam/CMetricMapBuilder.h>
00032 #include <mrpt/slam/CMultiMetricMapPDF.h>
00033 #include <mrpt/slam/CMultiMetricMap.h>
00034 
00035 #include <mrpt/bayes/CParticleFilter.h>
00036 #include <mrpt/bayes/CParticleFilterCapable.h>
00037 #include <mrpt/utils/CLoadableOptions.h>
00038 #include <mrpt/utils/safe_pointers.h>
00039 
00040 #include <mrpt/slam/link_pragmas.h>
00041 
00042 namespace mrpt
00043 {
00044 namespace slam
00045 {
00046         /** This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM).
00047          *   Internally, the list of particles, each containing a hypothesis for the robot path plus its associated
00048          *    metric map, is stored in an object of class CMultiMetricMapPDF.
00049          *
00050          *  This class processes robot actions and observations sequentially (through the method CMetricMapBuilderRBPF::processActionObservation)
00051          *   and exploits the generic design of metric map classes in MRPT to deal with any number and combination of maps simultaneously: the likelihood
00052          *   of observations is the product of the likelihood in the different maps, etc.
00053          *
00054          *   A number of particle filter methods are implemented as well, by selecting the appropriate values in TConstructionOptions::PF_options.
00055          *   Not all the PF algorithms are implemented for all kinds of maps.
00056          *
00057      *  For an example of usage, check the application "rbpf-slam", in "apps/RBPF-SLAM". See also the <a href="http://www.mrpt.org/Application:RBPF-SLAM">wiki page</a>.
00058          *
00059          *  \note Since MRPT 0.7.2, the new variables "localizeLinDistance,localizeAngDistance" are introduced to provide a way to update the robot pose at a different rate than the map is updated.
00060          *  \note Since MRPT 0.7.1 the semantics of the parameters "insertionLinDistance" and "insertionAngDistance" changes: the entire RBFP is now NOT updated unless odometry increments surpass the threshold (previously, only the map was NOT updated). This is done to gain efficiency.
00061          *  \note Since MRPT 0.6.2 this class implements full 6D SLAM. Previous versions worked in 2D + heading only.
00062          *
00063          * \sa CMetricMap
00064          */
00065         class SLAM_IMPEXP CMetricMapBuilderRBPF : public CMetricMapBuilder
00066         {
00067         public:
00068                 /** The map PDF: It includes a path and associated map for each particle.
00069                   */
00070                 CMultiMetricMapPDF                      mapPDF;
00071 
00072         protected:
00073         /** The configuration of the particle filter:
00074           */
00075                 bayes::CParticleFilter::TParticleFilterOptions  m_PF_options;
00076 
00077                 /** Distances (linear and angular) for inserting a new observation into the map.
00078                   */
00079                 float   insertionLinDistance,insertionAngDistance;
00080 
00081                 /** Distances (linear and angular) for updating the robot pose estimate (and particles weighs, if applicable).
00082                   */
00083                 float   localizeLinDistance,localizeAngDistance;
00084 
00085 
00086                 mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization;      //!< Traveled distance since last localization update
00087                 mrpt::poses::CPose3D                    odoIncrementSinceLastMapUpdate;         //!< Traveled distance since last map update
00088 
00089                 /** A buffer: memory is actually hold within "mapPDF".
00090                   */
00091                 non_copiable_ptr<CMultiMetricMap>  currentMetricMapEstimation;
00092 
00093         public:
00094 
00095                 /** Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
00096                   */
00097                 struct SLAM_IMPEXP TConstructionOptions : public utils::CLoadableOptions
00098                 {
00099                 public:
00100                         /** Constructor
00101                           */
00102                         TConstructionOptions();
00103 
00104                         /** See utils::CLoadableOptions
00105                           */
00106                         void  loadFromConfigFile(
00107                                 const mrpt::utils::CConfigFileBase  &source,
00108                                 const std::string &section);
00109 
00110                         /** See utils::CLoadableOptions
00111                           */
00112                         void  dumpToTextStream(CStream  &out) const;
00113 
00114                         float   insertionLinDistance;
00115                         float   insertionAngDistance;
00116 
00117                         float   localizeLinDistance;
00118                         float   localizeAngDistance;
00119 
00120                         bayes::CParticleFilter::TParticleFilterOptions  PF_options;
00121 
00122                         TSetOfMetricMapInitializers                                     mapsInitializers;
00123                         CMultiMetricMapPDF::TPredictionParams           predictionOptions;
00124                 };
00125 
00126                 /** Constructor.
00127                  */
00128                 CMetricMapBuilderRBPF( const TConstructionOptions &initializationOptions );
00129 
00130                 /** Destructor.
00131                  */
00132                 virtual ~CMetricMapBuilderRBPF( );
00133 
00134                 /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map.
00135                   */
00136                 void  initialize(
00137                                 const CSimpleMap                &initialMap  = CSimpleMap(),
00138                                 CPosePDF                                        *x0 = NULL
00139                                 );
00140 
00141                 /** Clear all elements of the maps.
00142                   */
00143                 void  clear();
00144 
00145                 /** Returns a copy of the current best pose estimation as a pose PDF.
00146                   */
00147                 CPose3DPDFPtr  getCurrentPoseEstimation() const;
00148 
00149                 /** Returns the current most-likely path estimation (the path associated to the most likely particle).
00150                   */
00151                 void  getCurrentMostLikelyPath( std::deque<TPose3D> &outPath ) const;
00152 
00153                 /** Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description.
00154                  *  \param action The incremental 2D pose change in the robot pose. This value is deterministic.
00155                  *      \param observations The set of observations that robot senses at the new pose.
00156                  *  Statistics will be saved to statsLastIteration
00157                  */
00158                 void  processActionObservation(
00159                                         CActionCollection       &action,
00160                                         CSensoryFrame           &observations );
00161 
00162                 /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
00163                   */
00164                 void  getCurrentlyBuiltMap(CSimpleMap &out_map) const;
00165 
00166                 /** Returns the map built so far. NOTE that for efficiency a pointer to the internal object is passed, DO NOT delete nor modify the object in any way, if desired, make a copy of ir with "duplicate()".
00167                   */
00168                 CMultiMetricMap*   getCurrentlyBuiltMetricMap();
00169 
00170                 /** Returns just how many sensory-frames are stored in the currently build map.
00171                   */
00172                 unsigned int  getCurrentlyBuiltMapSize();
00173 
00174                 /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
00175                   * \param file The output file name
00176                   * \param formatEMF_BMP Output format = true:EMF, false:BMP
00177                   */
00178                 void  saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true);
00179 
00180                 /** A usefull method for debugging: draws the current map and path hypotheses to a CCanvas
00181                   */
00182                 void  drawCurrentEstimationToImage( utils::CCanvas *img );
00183 
00184                 /** A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively).
00185                   */
00186                 void  saveCurrentPathEstimationToTextFile( std::string  fil );
00187 
00188                 double  getCurrentJointEntropy();
00189 
00190                 /** This structure will hold stats after each execution of processActionObservation
00191                   */
00192                 struct SLAM_IMPEXP TStats
00193                 {
00194                         TStats() :
00195                                 observationsInserted(false)
00196                         { }
00197 
00198                         /** Whether the SF has been inserted in the metric maps. */
00199                         bool    observationsInserted;
00200 
00201                 };
00202 
00203 
00204                 /** This structure will hold stats after each execution of processActionObservation
00205                   */
00206                 TStats          m_statsLastIteration;
00207 
00208         }; // End of class def.
00209 
00210         } // End of namespace
00211 } // End of namespace
00212 
00213 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011