00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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 namespace mrpt 00041 { 00042 namespace slam 00043 { 00044 /** This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM). 00045 * Internally, the list of particles, each containing a hypothesis for the robot path plus its associated 00046 * metric map, is stored in an object of class CMultiMetricMapPDF. 00047 * 00048 * This class processes robot actions and observations sequentially (through the method CMetricMapBuilderRBPF::processActionObservation) 00049 * and exploits the generic design of metric map classes in MRPT to deal with any number and combination of maps simultaneously: the likelihood 00050 * of observations is the product of the likelihood in the different maps, etc. 00051 * 00052 * A number of particle filter methods are implemented as well, by selecting the appropriate values in TConstructionOptions::PF_options. 00053 * Not all the PF algorithms are implemented for all kinds of maps. 00054 * 00055 * For an example of usage, check the application "rbpf-slam", in "apps/RBPF-SLAM". See also the <a href="http://babel.isa.uma.es/mrpt/index.php/Application:RBPF-SLAM">wiki page</a>. 00056 * 00057 * \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. 00058 * \note Since MRPT 0.6.2 this class implements full 6D SLAM. Previous versions worked in 2D + heading only. 00059 * 00060 * \sa CMetricMap 00061 */ 00062 class MRPTDLLIMPEXP CMetricMapBuilderRBPF : public CMetricMapBuilder 00063 { 00064 public: 00065 /** The map PDF: It includes a path and associated map for each particle. 00066 */ 00067 CMultiMetricMapPDF mapPDF; 00068 00069 protected: 00070 /** The configuration of the particle filter: 00071 */ 00072 bayes::CParticleFilter::TParticleFilterOptions m_PF_options; 00073 00074 /** Distances (linear and angular) for inserting a new observation into the map. 00075 */ 00076 float insertionLinDistance,insertionAngDistance; 00077 00078 /** Traveled distance since last processing of the PF. */ 00079 mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLast; //Replaces old: float linDistSinceLast,angDistSinceLast; 00080 00081 /** A buffer: memory is actually hold within "mapPDF". 00082 */ 00083 non_copiable_ptr<CMultiMetricMap> currentMetricMapEstimation; 00084 00085 public: 00086 00087 /** Options for building a CMetricMapBuilderRBPF object, passed to the constructor. 00088 */ 00089 struct MRPTDLLIMPEXP TConstructionOptions : public utils::CLoadableOptions 00090 { 00091 public: 00092 /** Constructor 00093 */ 00094 TConstructionOptions(); 00095 00096 /** See utils::CLoadableOptions 00097 */ 00098 void loadFromConfigFile( 00099 const mrpt::utils::CConfigFileBase &source, 00100 const std::string §ion); 00101 00102 /** See utils::CLoadableOptions 00103 */ 00104 void dumpToTextStream(CStream &out) const; 00105 00106 float insertionLinDistance; 00107 float insertionAngDistance; 00108 00109 bayes::CParticleFilter::TParticleFilterOptions PF_options; 00110 00111 TSetOfMetricMapInitializers mapsInitializers; 00112 CMultiMetricMapPDF::TPredictionParams predictionOptions; 00113 }; 00114 00115 /** Constructor. 00116 */ 00117 CMetricMapBuilderRBPF( const TConstructionOptions &initializationOptions ); 00118 00119 /** Destructor. 00120 */ 00121 virtual ~CMetricMapBuilderRBPF( ); 00122 00123 /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map. 00124 */ 00125 void initialize( 00126 CSensFrameProbSequence &initialMap, 00127 CPosePDF *x0 = NULL 00128 ); 00129 00130 /** Clear all elements of the maps. 00131 */ 00132 void clear(); 00133 00134 /** Returns a copy of the current best pose estimation as a pose PDF. 00135 */ 00136 CPose3DPDFPtr getCurrentPoseEstimation() const; 00137 00138 /** Returns the current most-likely path estimation (the path associated to the most likely particle). 00139 * This version ignore any 3D pose components. Use the full CPose3D-based version if full 3D data is needed. 00140 */ 00141 void getCurrentMostLikelyPath( std::deque<CPose2D> &outPath ) const; 00142 00143 /** Returns the current most-likely path estimation (the path associated to the most likely particle). 00144 */ 00145 void getCurrentMostLikelyPath( std::deque<TPose3D> &outPath ) const; 00146 00147 /** 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. 00148 * \param action The incremental 2D pose change in the robot pose. This value is deterministic. 00149 * \param observations The set of observations that robot senses at the new pose. 00150 * Statistics will be saved to statsLastIteration 00151 */ 00152 void processActionObservation( 00153 CActionCollection &action, 00154 CSensoryFrame &observations ); 00155 00156 /** Fills "out_map" with the set of "poses"-"sensorial frames", thus the so far built map. 00157 */ 00158 void getCurrentlyBuiltMap(CSensFrameProbSequence &out_map) const; 00159 00160 /** 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()". 00161 */ 00162 CMultiMetricMap* getCurrentlyBuiltMetricMap(); 00163 00164 /** Returns just how many sensorial frames are stored in the currently build map. 00165 */ 00166 unsigned int getCurrentlyBuiltMapSize(); 00167 00168 /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file. 00169 * \param file The output file name 00170 * \param formatEMF_BMP Output format = true:EMF, false:BMP 00171 */ 00172 void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true); 00173 00174 /** A usefull method for debugging: draws the current map and path hypotheses to a CCanvas 00175 */ 00176 void drawCurrentEstimationToImage( utils::CCanvas *img ); 00177 00178 /** 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). 00179 */ 00180 void saveCurrentPathEstimationToTextFile( std::string fil ); 00181 00182 double getCurrentJointEntropy(); 00183 00184 /** This structure will hold stats after each execution of processActionObservation 00185 */ 00186 struct MRPTDLLIMPEXP TStats 00187 { 00188 TStats() : 00189 observationsInserted(false) 00190 { } 00191 00192 /** Whether the SF has been inserted in the metric maps. */ 00193 bool observationsInserted; 00194 00195 }; 00196 00197 00198 /** This structure will hold stats after each execution of processActionObservation 00199 */ 00200 TStats m_statsLastIteration; 00201 00202 }; // End of class def. 00203 00204 } // End of namespace 00205 } // End of namespace 00206 00207 #endif
Page generated by Doxygen 1.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009 |