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 00029 #ifndef CHeightGridMap2D_H 00030 #define CHeightGridMap2D_H 00031 00032 #include <mrpt/utils/CDynamicGrid.h> 00033 #include <mrpt/utils/CSerializable.h> 00034 #include <mrpt/math/CMatrixD.h> 00035 #include <mrpt/system/os.h> 00036 #include <mrpt/utils/CLoadableOptions.h> 00037 #include <mrpt/utils/stl_extensions.h> 00038 00039 #include <mrpt/slam/CMetricMap.h> 00040 00041 namespace mrpt 00042 { 00043 namespace poses 00044 { 00045 class CPose2D; 00046 } 00047 namespace slam 00048 { 00049 using namespace mrpt::utils; 00050 00051 class CObservation; 00052 00053 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CHeightGridMap2D, CMetricMap ) 00054 00055 /** The contents of each cell in a CHeightGridMap2D map. 00056 **/ 00057 struct MRPTDLLIMPEXP THeightGridmapCell 00058 { 00059 /** Constructor 00060 */ 00061 THeightGridmapCell() : h(0), w(0), history_Zs() 00062 {} 00063 00064 /** The current average height (in meters). 00065 */ 00066 float h; 00067 00068 /** The current standard deviation of the height (in meters). 00069 */ 00070 float var; 00071 00072 /** Auxiliary variable for storing the incremental mean value (in meters). 00073 */ 00074 float u; 00075 00076 /** Auxiliary (in meters). 00077 */ 00078 float v; 00079 00080 00081 /** [mrSimpleAverage model] The accumulated weight: initially zero if un-observed, increased by one for each observation. 00082 */ 00083 uint32_t w; 00084 00085 /** [mrSlidingWindow model] The history of indiviudal heights. 00086 */ 00087 std::multimap<mrpt::system::TTimeStamp,float> history_Zs; 00088 }; 00089 00090 /** A mesh representation of a surface which keeps the estimated height for each (x,y) location. 00091 * Important implemented features are the insertion of 2D laser scans (from arbitrary 6D poses) and the exportation as 3D scenes. 00092 * 00093 * Each cell contains the up-to-date average height from measured falling in that cell. Two algorithms can be used here: 00094 * - mrSimpleAverage: Each cell only stores the current average value. 00095 * - mrSlidingWindow: Each cell keeps the average and all the individual measurements, so the can be removed by time using CHeightGridMap2D::removeObservationsByTimestamp . Do not use this model unless really required, since it's more memory demanding. 00096 */ 00097 class MRPTDLLIMPEXP CHeightGridMap2D : public CMetricMap, public utils::CDynamicGrid<THeightGridmapCell> 00098 { 00099 // This must be added to any CSerializable derived class: 00100 DEFINE_SERIALIZABLE( CHeightGridMap2D ) 00101 public: 00102 00103 float cell2float(const THeightGridmapCell& c) const 00104 { 00105 return float(c.h); 00106 } 00107 00108 /** The type of map representation to be used. 00109 * See mrpt::slam::CHeightGridMap2D for discussion. 00110 */ 00111 enum TMapRepresentation 00112 { 00113 mrSimpleAverage = 0, 00114 mrSlidingWindow 00115 }; 00116 00117 00118 /** Constructor 00119 */ 00120 CHeightGridMap2D( 00121 TMapRepresentation mapType = mrSimpleAverage, 00122 float x_min = -2, 00123 float x_max = 2, 00124 float y_min = -2, 00125 float y_max = 2, 00126 float resolution = 0.1 00127 ); 00128 00129 /** Erase all the contents of the map 00130 */ 00131 void clear(); 00132 00133 /** Returns true if the map is empty/no observation has been inserted. 00134 */ 00135 bool isEmpty() const; 00136 00137 /** Insert the observation information into this map. This method must be implemented 00138 * in derived classes. 00139 * \param obs The observation 00140 * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg) 00141 * 00142 * \sa CObservation::insertObservationInto 00143 */ 00144 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00145 00146 /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map. 00147 * 00148 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00149 * \param obs The observation. 00150 * \return This method returns a likelihood in the range [0,1]. 00151 * 00152 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00153 */ 00154 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 00155 00156 /** Remove all the points from the average of each cell which were inserted at exactly the given timestamp. 00157 * \return The number of points removed. 00158 */ 00159 size_t removeObservationsByTimestamp( const mrpt::system::TTimeStamp &tim ); 00160 00161 /** Parameters related with inserting observations into the map. 00162 */ 00163 struct MRPTDLLIMPEXP TInsertionOptions : public utils::CLoadableOptions 00164 { 00165 /** Default values loader: 00166 */ 00167 TInsertionOptions(); 00168 00169 /** See utils::CLoadableOptions 00170 */ 00171 void loadFromConfigFile( 00172 const mrpt::utils::CConfigFileBase &source, 00173 const std::string §ion); 00174 00175 /** See utils::CLoadableOptions 00176 */ 00177 void dumpToTextStream(CStream &out) const; 00178 00179 /** Wether to perform filtering by z-coordinate (default=false): coordinates are always RELATIVE to the robot for this filter. 00180 */ 00181 bool filterByHeight; 00182 00183 /** Only when filterByHeight is true: coordinates are always RELATIVE to the robot for this filter. 00184 */ 00185 float z_min,z_max; 00186 00187 float minDistBetweenPointsWhenInserting; //!< When inserting a scan, a point cloud is first created with this minimum distance between the 3D points (default=0). 00188 00189 } insertionOptions; 00190 00191 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00192 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00193 * \param otherMap [IN] The other map to compute the matching with. 00194 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00195 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00196 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00197 * 00198 * \return The matching ratio [0,1] 00199 * \sa computeMatchingWith2D 00200 */ 00201 float compute3DMatchingRatio( 00202 const CMetricMap *otherMap, 00203 const CPose3D &otherMapPose, 00204 float minDistForCorr = 0.10f, 00205 float minMahaDistForCorr = 2.0f 00206 ) const; 00207 00208 /** The implementation in this class just calls all the corresponding method of the contained metric maps. 00209 */ 00210 void saveMetricMapRepresentationToFile( 00211 const std::string &filNamePrefix 00212 ) const; 00213 00214 /** Returns a 3D object representing the map. 00215 */ 00216 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00217 00218 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00219 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00220 */ 00221 void auxParticleFilterCleanUp(); 00222 00223 /** Return the type of the gas distribution map, according to parameters passed on construction. 00224 */ 00225 TMapRepresentation getMapType(); 00226 00227 protected: 00228 00229 /** The map representation type of this map. 00230 */ 00231 TMapRepresentation m_mapType; 00232 00233 00234 }; 00235 00236 00237 } // End of namespace 00238 } // End of namespace 00239 00240 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:02:22 EDT 2009 |