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 CMetricMapBuilderICP_H 00029 #define CMetricMapBuilderICP_H 00030 00031 #include <mrpt/slam/CMetricMapBuilder.h> 00032 #include <mrpt/slam/CICP.h> 00033 00034 namespace mrpt 00035 { 00036 namespace slam 00037 { 00038 /** A class for very simple 2D SLAM based on ICP. This is a non-probabilistic pose tracking algorithm. 00039 * Map are stored as in files as binary dumps of "mrpt::slam::CSensFrameProbSequence" objects. The methods are 00040 * thread-safe. 00041 */ 00042 class MRPTDLLIMPEXP CMetricMapBuilderICP : public CMetricMapBuilder 00043 { 00044 public: 00045 /** Constructor. 00046 */ 00047 CMetricMapBuilderICP( 00048 TSetOfMetricMapInitializers *mapInitializers, 00049 float insertionLinDistance = 1.0f, 00050 float insertionAngDistance = DEG2RAD(30), 00051 CICP::TConfigParams *icpParams = NULL ); 00052 00053 /** Destructor: 00054 */ 00055 virtual ~CMetricMapBuilderICP(); 00056 00057 /** Algorithm configuration params 00058 */ 00059 struct MRPTDLLIMPEXP TConfigParams 00060 { 00061 /** Initializer 00062 */ 00063 TConfigParams () : matchAgainstTheGrid( false ) 00064 { 00065 00066 } 00067 00068 /** Match against the occupancy grid or the points map? The former is quicker but less precise. 00069 */ 00070 bool matchAgainstTheGrid; 00071 00072 } ICP_options; 00073 00074 CICP::TConfigParams ICP_params; 00075 00076 /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map. 00077 */ 00078 void initialize( 00079 CSensFrameProbSequence &initialMap, 00080 CPosePDF *x0 = NULL 00081 ); 00082 00083 /** Returns a copy of the current best pose estimation as a pose PDF. 00084 */ 00085 CPose3DPDFPtr getCurrentPoseEstimation() const; 00086 00087 /** 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. 00088 */ 00089 void setCurrentMapFile( const char *mapFile ); 00090 00091 /** 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. 00092 * \param action The estimation of the incremental pose change in the robot pose. 00093 * \param in_SF The set of observations that robot senses at the new pose. 00094 * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options 00095 */ 00096 void processActionObservation( 00097 CActionCollection &action, 00098 CSensoryFrame &in_SF ); 00099 00100 00101 /** Fills "out_map" with the set of "poses"-"sensorial frames", thus the so far built map. 00102 */ 00103 void getCurrentlyBuiltMap(CSensFrameProbSequence &out_map) const; 00104 00105 00106 /** (DEPRECATED) Returns the 2D points of current local map: This is for backward compatibility with the non-BABEL GUI, 00107 * should be removed when a new GUI integrated into BABEL becomes available. 00108 */ 00109 void getCurrentMapPoints( vector_float &x, vector_float &y); 00110 00111 /** 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()". 00112 */ 00113 CMultiMetricMap* getCurrentlyBuiltMetricMap(); 00114 00115 /** Returns just how many sensorial frames are stored in the currently build map. 00116 */ 00117 unsigned int getCurrentlyBuiltMapSize(); 00118 00119 /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file. 00120 * \param file The output file name 00121 * \param formatEMF_BMP Output format = true:EMF, false:BMP 00122 */ 00123 void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true); 00124 00125 private: 00126 /** The set of observations that leads to current map: 00127 */ 00128 CSensFrameProbSequence SF_Poses_seq; 00129 00130 /** The metric map representation as a points map: 00131 */ 00132 CMultiMetricMap metricMap; 00133 00134 /** Is map updating enabled? 00135 */ 00136 //bool mapUpdateEnabled; 00137 00138 /** To always insert the first observation: 00139 */ 00140 bool isTheFirstObservation; 00141 00142 /** Current map file. 00143 */ 00144 std::string currentMapFile; 00145 00146 /** The pose estimation by the alignment algorithm (ICP). 00147 */ 00148 CPosePDFGaussian estPose; 00149 00150 /** The threshold [0,1] for map update, based on good localization and new information. 00151 */ 00152 //float newInfoUpdateThreshold; 00153 00154 /** The estimated robot path: 00155 */ 00156 std::deque<CPose2D> estRobotPath; 00157 00158 /** Distances (linear and angular) for inserting a new observation into the map. 00159 */ 00160 float insertionLinDistance,insertionAngDistance; 00161 00162 /** Traveled distances from last insertion(map update): They are reset at each map update. 00163 * \sa insertionLinDistance, insertionAngDistance 00164 */ 00165 float linDistSinceLast,angDistSinceLast; 00166 00167 }; 00168 00169 } // End of namespace 00170 } // End of namespace 00171 00172 #endif
Page generated by Doxygen 1.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009 |