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