00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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.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. 00058 * \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. 00059 * \note Since MRPT 0.6.2 this class implements full 6D SLAM. Previous versions worked in 2D + heading only. 00060 * 00061 * \sa CMetricMap 00062 */ 00063 class MRPTDLLIMPEXP CMetricMapBuilderRBPF : public CMetricMapBuilder 00064 { 00065 public: 00066 /** The map PDF: It includes a path and associated map for each particle. 00067 */ 00068 CMultiMetricMapPDF mapPDF; 00069 00070 protected: 00071 /** The configuration of the particle filter: 00072 */ 00073 bayes::CParticleFilter::TParticleFilterOptions m_PF_options; 00074 00075 /** Distances (linear and angular) for inserting a new observation into the map. 00076 */ 00077 float insertionLinDistance,insertionAngDistance; 00078 00079 /** Distances (linear and angular) for updating the robot pose estimate (and particles weighs, if applicable). 00080 */ 00081 float localizeLinDistance,localizeAngDistance; 00082 00083 00084 mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization; //!< Traveled distance since last localization update 00085 mrpt::poses::CPose3D odoIncrementSinceLastMapUpdate; //!< Traveled distance since last map update 00086 00087 /** A buffer: memory is actually hold within "mapPDF". 00088 */ 00089 non_copiable_ptr<CMultiMetricMap> currentMetricMapEstimation; 00090 00091 public: 00092 00093 /** Options for building a CMetricMapBuilderRBPF object, passed to the constructor. 00094 */ 00095 struct MRPTDLLIMPEXP TConstructionOptions : public utils::CLoadableOptions 00096 { 00097 public: 00098 /** Constructor 00099 */ 00100 TConstructionOptions(); 00101 00102 /** See utils::CLoadableOptions 00103 */ 00104 void loadFromConfigFile( 00105 const mrpt::utils::CConfigFileBase &source, 00106 const std::string §ion); 00107 00108 /** See utils::CLoadableOptions 00109 */ 00110 void dumpToTextStream(CStream &out) const; 00111 00112 float insertionLinDistance; 00113 float insertionAngDistance; 00114 00115 float localizeLinDistance; 00116 float localizeAngDistance; 00117 00118 bayes::CParticleFilter::TParticleFilterOptions PF_options; 00119 00120 TSetOfMetricMapInitializers mapsInitializers; 00121 CMultiMetricMapPDF::TPredictionParams predictionOptions; 00122 }; 00123 00124 /** Constructor. 00125 */ 00126 CMetricMapBuilderRBPF( const TConstructionOptions &initializationOptions ); 00127 00128 /** Destructor. 00129 */ 00130 virtual ~CMetricMapBuilderRBPF( ); 00131 00132 /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map. 00133 */ 00134 void initialize( 00135 CSensFrameProbSequence &initialMap, 00136 CPosePDF *x0 = NULL 00137 ); 00138 00139 /** Clear all elements of the maps. 00140 */ 00141 void clear(); 00142 00143 /** Returns a copy of the current best pose estimation as a pose PDF. 00144 */ 00145 CPose3DPDFPtr getCurrentPoseEstimation() const; 00146 00147 /** Returns the current most-likely path estimation (the path associated to the most likely particle). 00148 * This version ignore any 3D pose components. Use the full CPose3D-based version if full 3D data is needed. 00149 */ 00150 void getCurrentMostLikelyPath( std::deque<CPose2D> &outPath ) const; 00151 00152 /** Returns the current most-likely path estimation (the path associated to the most likely particle). 00153 */ 00154 void getCurrentMostLikelyPath( std::deque<TPose3D> &outPath ) const; 00155 00156 /** 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. 00157 * \param action The incremental 2D pose change in the robot pose. This value is deterministic. 00158 * \param observations The set of observations that robot senses at the new pose. 00159 * Statistics will be saved to statsLastIteration 00160 */ 00161 void processActionObservation( 00162 CActionCollection &action, 00163 CSensoryFrame &observations ); 00164 00165 /** Fills "out_map" with the set of "poses"-"sensorial frames", thus the so far built map. 00166 */ 00167 void getCurrentlyBuiltMap(CSensFrameProbSequence &out_map) const; 00168 00169 /** 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()". 00170 */ 00171 CMultiMetricMap* getCurrentlyBuiltMetricMap(); 00172 00173 /** Returns just how many sensorial frames are stored in the currently build map. 00174 */ 00175 unsigned int getCurrentlyBuiltMapSize(); 00176 00177 /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file. 00178 * \param file The output file name 00179 * \param formatEMF_BMP Output format = true:EMF, false:BMP 00180 */ 00181 void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true); 00182 00183 /** A usefull method for debugging: draws the current map and path hypotheses to a CCanvas 00184 */ 00185 void drawCurrentEstimationToImage( utils::CCanvas *img ); 00186 00187 /** 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). 00188 */ 00189 void saveCurrentPathEstimationToTextFile( std::string fil ); 00190 00191 double getCurrentJointEntropy(); 00192 00193 /** This structure will hold stats after each execution of processActionObservation 00194 */ 00195 struct MRPTDLLIMPEXP TStats 00196 { 00197 TStats() : 00198 observationsInserted(false) 00199 { } 00200 00201 /** Whether the SF has been inserted in the metric maps. */ 00202 bool observationsInserted; 00203 00204 }; 00205 00206 00207 /** This structure will hold stats after each execution of processActionObservation 00208 */ 00209 TStats m_statsLastIteration; 00210 00211 }; // End of class def. 00212 00213 } // End of namespace 00214 } // End of namespace 00215 00216 #endif
| Page generated by Doxygen 1.6.2 for MRPT 0.8.1 SVN:exported at Mon Feb 15 22:01:07 UTC 2010 |