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 CMetricMapBuilderICP_H 00029 #define CMetricMapBuilderICP_H 00030 00031 #include <mrpt/slam/CMetricMapBuilder.h> 00032 #include <mrpt/slam/CICP.h> 00033 #include <mrpt/poses/CRobot2DPoseEstimator.h> 00034 00035 #include <mrpt/slam/link_pragmas.h> 00036 00037 00038 namespace mrpt 00039 { 00040 namespace slam 00041 { 00042 /** A class for very simple 2D SLAM based on ICP. This is a non-probabilistic pose tracking algorithm. 00043 * Map are stored as in files as binary dumps of "mrpt::slam::CSimpleMap" objects. The methods are 00044 * thread-safe. 00045 */ 00046 class SLAM_IMPEXP CMetricMapBuilderICP : public CMetricMapBuilder 00047 { 00048 public: 00049 /** Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize". 00050 */ 00051 CMetricMapBuilderICP(); 00052 00053 /** *DEPRECATED* Constructor with arguments - use the default constructor, then set the parameters in the "ICP_options" member. 00054 * \param mapInitializers What maps to create (at least one points map and/or a grid map are needed). 00055 * \param insertionLinDistance Minimum robot linear displacement for a new observation to be inserted in the map. 00056 * \param insertionAngDistance Minimum robot angular displacement for a new observation to be inserted in the map. 00057 */ 00058 CMetricMapBuilderICP( 00059 TSetOfMetricMapInitializers *mapInitializers, 00060 float insertionLinDistance = 1.0f, 00061 float insertionAngDistance = DEG2RAD(30), 00062 CICP::TConfigParams *icpParams = NULL 00063 ); 00064 00065 /** Destructor: 00066 */ 00067 virtual ~CMetricMapBuilderICP(); 00068 00069 /** Algorithm configuration params 00070 */ 00071 struct SLAM_IMPEXP TConfigParams : public mrpt::utils::CLoadableOptions 00072 { 00073 /** Initializer */ 00074 TConfigParams (); 00075 virtual void loadFromConfigFile( 00076 const mrpt::utils::CConfigFileBase &source, 00077 const std::string §ion); 00078 virtual void dumpToTextStream( CStream &out) const; 00079 00080 /** (default:false) Match against the occupancy grid or the points map? The former is quicker but less precise. */ 00081 bool matchAgainstTheGrid; 00082 00083 double insertionLinDistance; //!< Minimum robot linear (m) displacement for a new observation to be inserted in the map. 00084 double insertionAngDistance; //!< Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be inserted in the map. 00085 double localizationLinDistance; //!< Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (otherwise, dead-reckon with odometry). 00086 double localizationAngDistance;//!< Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be used to do ICP-based localization (otherwise, dead-reckon with odometry). 00087 00088 double minICPgoodnessToAccept; //!< Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0.40) 00089 00090 /** What maps to create (at least one points map and/or a grid map are needed). 00091 * For the expected format in the .ini file when loaded with loadFromConfigFile(), see documentation of TSetOfMetricMapInitializers. 00092 */ 00093 TSetOfMetricMapInitializers mapInitializers; 00094 00095 }; 00096 00097 TConfigParams ICP_options; //!< Options for the ICP-SLAM application \sa ICP_params 00098 CICP::TConfigParams ICP_params; //!< Options for the ICP algorithm itself \sa ICP_options 00099 00100 /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map. 00101 * This method MUST be called if using the default constructor, after loading the configuration into ICP_options. In particular, TConfigParams::mapInitializers 00102 */ 00103 void initialize( 00104 const CSimpleMap &initialMap = CSimpleMap(), 00105 CPosePDF *x0 = NULL 00106 ); 00107 00108 /** Returns a copy of the current best pose estimation as a pose PDF. 00109 */ 00110 CPose3DPDFPtr getCurrentPoseEstimation() const; 00111 00112 /** Sets the "current map file", thus that map will be loaded if it exists or a new one will be created if it does not, and the updated map will be save to that file when destroying the object. 00113 */ 00114 void setCurrentMapFile( const char *mapFile ); 00115 00116 /** 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. 00117 * \param action The estimation of the incremental pose change in the robot pose. 00118 * \param in_SF The set of observations that robot senses at the new pose. 00119 * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options 00120 * \sa processObservation 00121 */ 00122 void processActionObservation( 00123 CActionCollection &action, 00124 CSensoryFrame &in_SF ); 00125 00126 /** The main method of this class: Process one odometry or sensor observation. 00127 The new entry point of the algorithm (the old one was processActionObservation, which now is a wrapper to 00128 this method). 00129 * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options 00130 */ 00131 void processObservation(const CObservationPtr &obs); 00132 00133 00134 /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map. 00135 */ 00136 void getCurrentlyBuiltMap(CSimpleMap &out_map) const; 00137 00138 00139 /** Returns the 2D points of current local map 00140 */ 00141 void getCurrentMapPoints( std::vector<float> &x, std::vector<float> &y); 00142 00143 /** 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()". 00144 */ 00145 CMultiMetricMap* getCurrentlyBuiltMetricMap(); 00146 00147 /** Returns just how many sensory-frames are stored in the currently build map. 00148 */ 00149 unsigned int getCurrentlyBuiltMapSize(); 00150 00151 /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file. 00152 * \param file The output file name 00153 * \param formatEMF_BMP Output format = true:EMF, false:BMP 00154 */ 00155 void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true); 00156 00157 private: 00158 /** The set of observations that leads to current map: 00159 */ 00160 CSimpleMap SF_Poses_seq; 00161 00162 /** The metric map representation as a points map: 00163 */ 00164 CMultiMetricMap metricMap; 00165 00166 /** Current map file. 00167 */ 00168 std::string currentMapFile; 00169 00170 /** The pose estimation by the alignment algorithm (ICP). */ 00171 mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst; //!< Last pose estimation (Mean) 00172 mrpt::math::CMatrixDouble33 m_lastPoseEst_cov; //!< Last pose estimation (covariance) 00173 00174 /** The estimated robot path: 00175 */ 00176 std::deque<mrpt::math::TPose2D> m_estRobotPath; 00177 mrpt::poses::CPose2D m_auxAccumOdometry; 00178 00179 /** Traveled distances from last map update / ICP-based localization. */ 00180 struct SLAM_IMPEXP TDist 00181 { 00182 TDist() : lin(0),ang(0) { } 00183 double lin; // meters 00184 double ang; // degrees 00185 }; 00186 TDist m_distSinceLastICP; 00187 std::map<std::string,TDist> m_distSinceLastInsertion; //!< Indexed by sensor label. 00188 bool m_there_has_been_an_odometry; 00189 void accumulateRobotDisplacementCounters(const CPose2D &Apose); 00190 00191 }; 00192 00193 } // End of namespace 00194 } // End of namespace 00195 00196 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011 |