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 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) 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 00089 /** A mesh representation of a surface which keeps the estimated height for each (x,y) location. 00090 * Important implemented features are the insertion of 2D laser scans (from arbitrary 6D poses) and the exportation as 3D scenes. 00091 * 00092 * Each cell contains the up-to-date average height from measured falling in that cell. Algorithms that can be used: 00093 * - mrSimpleAverage: Each cell only stores the current average value. 00094 */ 00095 class MAPS_IMPEXP CHeightGridMap2D : public CMetricMap, public utils::CDynamicGrid<THeightGridmapCell> 00096 { 00097 // This must be added to any CSerializable derived class: 00098 DEFINE_SERIALIZABLE( CHeightGridMap2D ) 00099 public: 00100 00101 /** Calls the base CMetricMap::clear 00102 * Declared here to avoid ambiguity between the two clear() in both base classes. 00103 */ 00104 inline void clear() { CMetricMap::clear(); } 00105 00106 float cell2float(const THeightGridmapCell& c) const 00107 { 00108 return float(c.h); 00109 } 00110 00111 /** The type of map representation to be used. 00112 * See mrpt::slam::CHeightGridMap2D for discussion. 00113 */ 00114 enum TMapRepresentation 00115 { 00116 mrSimpleAverage = 0 00117 // mrSlidingWindow 00118 }; 00119 00120 /** Constructor 00121 */ 00122 CHeightGridMap2D( 00123 TMapRepresentation mapType = mrSimpleAverage, 00124 float x_min = -2, 00125 float x_max = 2, 00126 float y_min = -2, 00127 float y_max = 2, 00128 float resolution = 0.1 00129 ); 00130 00131 /** Returns true if the map is empty/no observation has been inserted. 00132 */ 00133 bool isEmpty() const; 00134 00135 /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map. 00136 * 00137 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00138 * \param obs The observation. 00139 * \return This method returns a likelihood in the range [0,1]. 00140 * 00141 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00142 */ 00143 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 00144 00145 /** Parameters related with inserting observations into the map. 00146 */ 00147 struct MAPS_IMPEXP TInsertionOptions : public utils::CLoadableOptions 00148 { 00149 /** Default values loader: 00150 */ 00151 TInsertionOptions(); 00152 00153 /** See utils::CLoadableOptions 00154 */ 00155 void loadFromConfigFile( 00156 const mrpt::utils::CConfigFileBase &source, 00157 const std::string §ion); 00158 00159 /** See utils::CLoadableOptions 00160 */ 00161 void dumpToTextStream(CStream &out) const; 00162 00163 /** Wether to perform filtering by z-coordinate (default=false): coordinates are always RELATIVE to the robot for this filter. 00164 */ 00165 bool filterByHeight; 00166 00167 /** Only when filterByHeight is true: coordinates are always RELATIVE to the robot for this filter. 00168 */ 00169 float z_min,z_max; 00170 00171 float minDistBetweenPointsWhenInserting; //!< When inserting a scan, a point cloud is first created with this minimum distance between the 3D points (default=0). 00172 00173 } insertionOptions; 00174 00175 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00176 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00177 * \param otherMap [IN] The other map to compute the matching with. 00178 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00179 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00180 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00181 * 00182 * \return The matching ratio [0,1] 00183 * \sa computeMatchingWith2D 00184 */ 00185 float compute3DMatchingRatio( 00186 const CMetricMap *otherMap, 00187 const CPose3D &otherMapPose, 00188 float minDistForCorr = 0.10f, 00189 float minMahaDistForCorr = 2.0f 00190 ) const; 00191 00192 /** The implementation in this class just calls all the corresponding method of the contained metric maps. 00193 */ 00194 void saveMetricMapRepresentationToFile( 00195 const std::string &filNamePrefix 00196 ) const; 00197 00198 /** Returns a 3D object representing the map: by default, it will be a mrpt::opengl::CMesh object, unless 00199 * it is specified otherwise in mrpt:: 00200 */ 00201 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00202 00203 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00204 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00205 */ 00206 void auxParticleFilterCleanUp(); 00207 00208 /** Return the type of the gas distribution map, according to parameters passed on construction. 00209 */ 00210 TMapRepresentation getMapType(); 00211 00212 00213 /** Gets the intersection between a 3D line and a Height Grid map (taking into account the different heights of each individual cell). 00214 */ 00215 bool intersectLine3D(const TLine3D &r1, TObject3D &obj) const; 00216 00217 /** Computes the minimum and maximum height in the grid. 00218 * \return False if there is no observed cell yet. 00219 */ 00220 bool getMinMaxHeight(float &z_min, float &z_max) const; 00221 00222 /** Return the number of cells with at least one height data inserted. */ 00223 size_t countObservedCells() const; 00224 00225 protected: 00226 00227 /** The map representation type of this map. 00228 */ 00229 TMapRepresentation m_mapType; 00230 00231 /** Erase all the contents of the map 00232 */ 00233 virtual void internal_clear(); 00234 00235 /** Insert the observation information into this map. This method must be implemented 00236 * in derived classes. 00237 * \param obs The observation 00238 * \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) 00239 * 00240 * \sa CObservation::insertObservationInto 00241 */ 00242 virtual bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00243 00244 }; 00245 00246 00247 } // End of namespace 00248 00249 namespace global_settings 00250 { 00251 /** If set to true (default), mrpt::slam::CHeightGridMap2D will be exported as a opengl::CMesh, otherwise, as a opengl::CPointCloudColoured 00252 * Affects to: 00253 * - CHeightGridMap2D::getAs3DObject 00254 */ 00255 extern MAPS_IMPEXP bool HEIGHTGRIDMAP_EXPORT3D_AS_MESH; 00256 } 00257 00258 // Specializations MUST occur at the same namespace: 00259 namespace utils 00260 { 00261 template <> 00262 struct TEnumTypeFiller<slam::CHeightGridMap2D::TMapRepresentation> 00263 { 00264 typedef slam::CHeightGridMap2D::TMapRepresentation enum_t; 00265 static void fill(bimap<enum_t,std::string> &m_map) 00266 { 00267 m_map.insert(slam::CHeightGridMap2D::mrSimpleAverage, "mrSimpleAverage"); 00268 } 00269 }; 00270 } // End of namespace 00271 00272 } // End of namespace 00273 00274 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011 |