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 §ion); 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 |