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 COccupancyGridMap2D_H 00030 #define COccupancyGridMap2D_H 00031 00032 #include <mrpt/utils/CSerializable.h> 00033 #include <mrpt/utils/CLoadableOptions.h> 00034 #include <mrpt/utils/CImage.h> 00035 #include <mrpt/utils/CImageFloat.h> 00036 #include <mrpt/slam/CMetricMap.h> 00037 #include <mrpt/utils/TMatchingPair.h> 00038 00039 #include <mrpt/maps/link_pragmas.h> 00040 00041 #include <mrpt/utils/safe_pointers.h> 00042 00043 #include <mrpt/config.h> 00044 00045 #if !defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && !defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS) 00046 #error One of OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS or OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS must be defined. 00047 #endif 00048 00049 #if defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS) 00050 #error Only one of OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS or OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS must be defined at a time. 00051 #endif 00052 00053 // Discrete to float conversion factors: 00054 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS 00055 /** The min/max values of the integer cell type, eg.[0,255] or [0,65535] 00056 */ 00057 #define OCCGRID_CELLTYPE_MIN (-32768) 00058 #define OCCGRID_CELLTYPE_MAX (32767) 00059 00060 #define OCCGRID_P2LTABLE_SIZE (32767) 00061 #else 00062 /** The min/max values of the integer cell type, eg.[0,255] or [0,65535] 00063 */ 00064 #define OCCGRID_CELLTYPE_MIN (-128) 00065 #define OCCGRID_CELLTYPE_MAX (127) 00066 00067 #define OCCGRID_P2LTABLE_SIZE (128) 00068 #endif 00069 00070 // The factor for converting log2-odds into integers: 00071 #define OCCGRID_LOGODD_K (16) 00072 #define OCCGRID_LOGODD_K_INV (1.0f/OCCGRID_LOGODD_K) 00073 00074 00075 namespace mrpt 00076 { 00077 namespace poses { class CPose2D; } 00078 namespace slam 00079 { 00080 using namespace mrpt::poses; 00081 using namespace mrpt::utils; 00082 00083 class CObservation2DRangeScan; 00084 class CObservationRange; 00085 class CObservation; 00086 class CPointsMap; 00087 00088 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( COccupancyGridMap2D, CMetricMap, MAPS_IMPEXP ) 00089 00090 /** A class for storing an occupancy grid map. 00091 * COccupancyGridMap2D is a class for storing a metric map 00092 * representation in the form of a probabilistic occupancy 00093 * grid map: value of 0 means certainly occupied, 1 means 00094 * a certainly empty cell. Initially 0.5 means uncertainty. 00095 * 00096 * The cells keep the log-odd representation of probabilities instead of the probabilities themselves. 00097 * More details can be found at http://www.mrpt.org/Occupancy_Grids 00098 * 00099 * The algorithm for updating the grid from a laser scanner can optionally take into account the progressive widening of the beams, as 00100 * described in the <a href="http://www.mrpt.org/Occupancy_Grids" > wiki </a> (this feature was introduced in MRPT 0.6.4). 00101 * 00102 * Some implemented methods are: 00103 * - Update of individual cells 00104 * - Insertion of observations 00105 * - Voronoi diagram and critical points 00106 * - Saving and loading from/to a bitmap 00107 * - Laser scans simulation for the map contents 00108 * - Entropy and information methods (See computeEntropy) 00109 * 00110 **/ 00111 class MAPS_IMPEXP COccupancyGridMap2D : public CMetricMap 00112 { 00113 // This must be added to any CSerializable derived class: 00114 DEFINE_SERIALIZABLE( COccupancyGridMap2D ) 00115 00116 public: 00117 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS 00118 /** The type of the map cells: 00119 */ 00120 typedef int8_t cellType; 00121 typedef uint8_t cellTypeUnsigned; 00122 #endif 00123 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS 00124 /** The type of the map cells: 00125 */ 00126 typedef int16_t cellType; 00127 typedef uint16_t cellTypeUnsigned; 00128 #endif 00129 00130 protected: 00131 00132 friend class CMultiMetricMap; 00133 friend class CMultiMetricMapPDF; 00134 00135 /** This is the buffer for storing the cells.In this dynamic 00136 * size buffer are stored the cell values as 00137 * "bytes", stored row by row, from left to right cells. 00138 */ 00139 std::vector<cellType> map; 00140 00141 /** The size of the grid in cells. 00142 */ 00143 uint32_t size_x,size_y; 00144 00145 /** The limits of the grid in "units" (meters). 00146 */ 00147 float x_min,x_max,y_min,y_max; 00148 00149 /** Cell size, i.e. resolution of the grid map. 00150 */ 00151 float resolution; 00152 00153 /** These are auxiliary variables to speed up the computation of observation likelihood values for LF method among others, at a high cost in memory (see TLikelihoodOptions::enableLikelihoodCache). 00154 */ 00155 std::vector<double> precomputedLikelihood; 00156 bool precomputedLikelihoodToBeRecomputed; 00157 00158 /** Used for Voronoi calculation.Same struct as "map", but contains 00159 * a "0" if not a basis point. 00160 */ 00161 std::vector<unsigned char> basis_map; 00162 00163 /** Used to store the Voronoi diagram.Same struct as "map", but 00164 * contains the distance of each cell to its closer obstacles 00165 * in 1/100 of "units", or 0 if not into the Voronoi diagram. 00166 */ 00167 std::vector<int> voronoi_diagram; 00168 00169 bool m_is_empty; //!< True upon construction; used by isEmpty() 00170 00171 virtual void OnPostSuccesfulInsertObs(const CObservation *); //!< See base class 00172 00173 /** The free-cells threshold used to compute the Voronoi diagram. 00174 */ 00175 float voroni_free_threshold; 00176 00177 /** Frees the dynamic memory buffers of map. 00178 */ 00179 void freeMap(); 00180 00181 /** Entropy computation internal function: 00182 */ 00183 static double H(double p); 00184 00185 /** Change the contents [0,1] of a cell, given its index. 00186 */ 00187 inline void setCell_nocheck(int x,int y,float value) 00188 { 00189 map[x+y*size_x]=p2l(value); 00190 } 00191 00192 /** Read the real valued [0,1] contents of a cell, given its index. 00193 */ 00194 inline float getCell_nocheck(int x,int y) const 00195 { 00196 return l2p(map[x+y*size_x]); 00197 } 00198 00199 /** Changes a cell by its absolute index (Do not use it normally) 00200 */ 00201 inline void setRawCell(unsigned int cellIndex, cellType b) 00202 { 00203 if (cellIndex<size_x*size_y) 00204 { 00205 map[cellIndex] = b; 00206 } 00207 } 00208 00209 /** Internally used to speed-up entropy calculation 00210 */ 00211 static std::vector<float> entropyTable; 00212 00213 /** A lookup table to compute occupancy probabilities in [0,1] from integer log-odds values in the cells, using \f$ p(m_{xy}) = \frac{1}{1+exp(-log_odd)} \f$. 00214 */ 00215 static std::vector<float> logoddsTable; 00216 00217 /** A lookup table to compute occupancy probabilities in the range [0,255] from integer log-odds values in the cells, using \f$ p(m_{xy}) = \frac{1}{1+exp(-log_odd)} \f$. 00218 * This is used to speed-up conversions to grayscale images. 00219 */ 00220 static std::vector<uint8_t> logoddsTable_255; 00221 00222 /** A pointer to the vector logoddsTable */ 00223 static float* logoddsTablePtr; 00224 00225 /** A pointer to the vector logoddsTable_255 */ 00226 static uint8_t* logoddsTable_255Ptr; 00227 00228 /** A lookup table for passing from float to log-odds as cellType. */ 00229 static std::vector<cellType> p2lTable; 00230 00231 /** A pointer to the vector p2lTable */ 00232 static cellType* p2lTablePtr; 00233 00234 00235 /** One of the methods that can be selected for implementing "computeObservationLikelihood" (This method is the Range-Scan Likelihood Consensus for gridmaps, see the ICRA2007 paper by Blanco et al.) 00236 */ 00237 double computeObservationLikelihood_Consensus( 00238 const CObservation *obs, 00239 const CPose2D &takenFrom ); 00240 00241 /** One of the methods that can be selected for implementing "computeObservationLikelihood" 00242 * TODO: This method is described in.... 00243 */ 00244 double computeObservationLikelihood_ConsensusOWA( 00245 const CObservation *obs, 00246 const CPose2D &takenFrom ); 00247 00248 /** One of the methods that can be selected for implementing "computeObservationLikelihood". 00249 */ 00250 double computeObservationLikelihood_CellsDifference( 00251 const CObservation *obs, 00252 const CPose2D &takenFrom ); 00253 00254 /** One of the methods that can be selected for implementing "computeObservationLikelihood". 00255 */ 00256 double computeObservationLikelihood_MI( 00257 const CObservation *obs, 00258 const CPose2D &takenFrom ); 00259 00260 /** One of the methods that can be selected for implementing "computeObservationLikelihood". 00261 */ 00262 double computeObservationLikelihood_rayTracing( 00263 const CObservation *obs, 00264 const CPose2D &takenFrom ); 00265 00266 /** One of the methods that can be selected for implementing "computeObservationLikelihood". 00267 */ 00268 double computeObservationLikelihood_likelihoodField_Thrun( 00269 const CObservation *obs, 00270 const CPose2D &takenFrom ); 00271 00272 /** One of the methods that can be selected for implementing "computeObservationLikelihood". 00273 */ 00274 double computeObservationLikelihood_likelihoodField_II( 00275 const CObservation *obs, 00276 const CPose2D &takenFrom ); 00277 00278 /** Clear the map: It set all cells to their default occupancy value (0.5), without changing the resolution (the grid extension is reset to the default values). 00279 */ 00280 virtual void internal_clear( ); 00281 00282 /** Insert the observation information into this map. 00283 * 00284 * \param obs The observation 00285 * \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) 00286 * 00287 * After successfull execution, "lastObservationInsertionInfo" is updated. 00288 * 00289 * \sa insertionOptions, CObservation::insertObservationInto 00290 */ 00291 virtual bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00292 00293 public: 00294 00295 /** Performs the Bayesian fusion of a new observation of a cell. 00296 * \sa updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free 00297 */ 00298 void updateCell(int x,int y, float v); 00299 00300 /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly. 00301 * This method increases the "occupancy-ness" of a cell, managing possible saturation. 00302 * \param x Cell index in X axis. 00303 * \param y Cell index in Y axis. 00304 * \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l. 00305 * \param thres This must be OCCGRID_CELLTYPE_MIN+logodd_obs 00306 * \sa updateCell, updateCell_fast_free 00307 */ 00308 inline static void updateCell_fast_occupied( 00309 const unsigned &x, 00310 const unsigned &y, 00311 const cellType &logodd_obs, 00312 const cellType &thres, 00313 cellType *mapArray, 00314 const unsigned &_size_x) 00315 { 00316 cellType *theCell = mapArray + (x+y*_size_x); 00317 if (*theCell > thres ) 00318 *theCell -= logodd_obs; 00319 else *theCell = OCCGRID_CELLTYPE_MIN; 00320 } 00321 00322 /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly. 00323 * This method increases the "occupancy-ness" of a cell, managing possible saturation. 00324 * \param theCell The cell to modify 00325 * \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l. 00326 * \param thres This must be OCCGRID_CELLTYPE_MIN+logodd_obs 00327 * \sa updateCell, updateCell_fast_free 00328 */ 00329 inline static void updateCell_fast_occupied( 00330 cellType *theCell, 00331 const cellType &logodd_obs, 00332 const cellType &thres ) 00333 { 00334 if (*theCell > thres ) 00335 *theCell -= logodd_obs; 00336 else *theCell = OCCGRID_CELLTYPE_MIN; 00337 } 00338 00339 /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly. 00340 * This method increases the "free-ness" of a cell, managing possible saturation. 00341 * \param x Cell index in X axis. 00342 * \param y Cell index in Y axis. 00343 * \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l. 00344 * \param thres This must be OCCGRID_CELLTYPE_MAX-logodd_obs 00345 * \sa updateCell_fast_occupied 00346 */ 00347 inline static void updateCell_fast_free( 00348 const unsigned &x, 00349 const unsigned &y, 00350 const cellType &logodd_obs, 00351 const cellType &thres, 00352 cellType *mapArray, 00353 const unsigned &_size_x) 00354 { 00355 cellType *theCell = mapArray + (x+y*_size_x); 00356 if (*theCell < thres ) 00357 *theCell += logodd_obs; 00358 else *theCell = OCCGRID_CELLTYPE_MAX; 00359 } 00360 00361 /** Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly. 00362 * This method increases the "free-ness" of a cell, managing possible saturation. 00363 * \param x Cell index in X axis. 00364 * \param y Cell index in Y axis. 00365 * \param logodd_obs Observation of the cell, in log-odd form as transformed by p2l. 00366 * \param thres This must be OCCGRID_CELLTYPE_MAX-logodd_obs 00367 * \sa updateCell_fast_occupied 00368 */ 00369 inline static void updateCell_fast_free( 00370 cellType *theCell, 00371 const cellType &logodd_obs, 00372 const cellType &thres) 00373 { 00374 if (*theCell < thres ) 00375 *theCell += logodd_obs; 00376 else *theCell = OCCGRID_CELLTYPE_MAX; 00377 } 00378 00379 /** An internal structure for storing data related to counting the new information apported by some observation. 00380 */ 00381 struct MAPS_IMPEXP TUpdateCellsInfoChangeOnly 00382 { 00383 TUpdateCellsInfoChangeOnly( bool enabled = false, 00384 double I_change = 0, 00385 int cellsUpdated=0) : enabled(enabled), 00386 I_change(I_change), 00387 cellsUpdated(cellsUpdated), 00388 laserRaysSkip(1) 00389 { 00390 } 00391 00392 /** If set to false (default), this struct is not used. Set to true only when measuring the info of an observation. 00393 */ 00394 bool enabled; 00395 00396 /** The cummulative change in Information: This is updated only from the "updateCell" method. 00397 */ 00398 double I_change; 00399 00400 /** The cummulative updated cells count: This is updated only from the "updateCell" method. 00401 */ 00402 int cellsUpdated; 00403 00404 /** In this mode, some laser rays can be skips to speep-up 00405 */ 00406 int laserRaysSkip; 00407 } updateInfoChangeOnly; 00408 00409 /** Constructor. 00410 */ 00411 COccupancyGridMap2D( float min_x = -20.0f, 00412 float max_x = 20.0f, 00413 float min_y = -20.0f, 00414 float max_y = 20.0f, 00415 float resolution = 0.05f 00416 ); 00417 00418 /** Fills all the cells with a default value. 00419 */ 00420 void fill(float default_value = 0.5f ); 00421 00422 /** Destructor. 00423 */ 00424 virtual ~COccupancyGridMap2D(); 00425 00426 /** Change the size of gridmap, erasing all its previous contents. 00427 * \param x_min The "x" coordinates of left most side of grid. 00428 * \param x_max The "x" coordinates of right most side of grid. 00429 * \param y_min The "y" coordinates of top most side of grid. 00430 * \param y_max The "y" coordinates of bottom most side of grid. 00431 * \param resolution The new size of cells. 00432 * \param default_value The value of cells, tipically 0.5. 00433 * \sa ResizeGrid 00434 */ 00435 void setSize(float x_min,float x_max,float y_min,float y_max,float resolution,float default_value = 0.5f); 00436 00437 /** Change the size of gridmap, maintaining previous contents. 00438 * \param new_x_min The "x" coordinates of new left most side of grid. 00439 * \param new_x_max The "x" coordinates of new right most side of grid. 00440 * \param new_y_min The "y" coordinates of new top most side of grid. 00441 * \param new_y_max The "y" coordinates of new bottom most side of grid. 00442 * \param new_cells_default_value The value of the new cells, tipically 0.5. 00443 * \param additionalMargin If set to true (default), an additional margin of a few meters will be added to the grid, ONLY if the new coordinates are larger than current ones. 00444 * \sa setSize 00445 */ 00446 void resizeGrid(float new_x_min,float new_x_max,float new_y_min,float new_y_max,float new_cells_default_value = 0.5f, bool additionalMargin = true) MRPT_NO_THROWS; 00447 00448 /** Returns the area of the gridmap, in square meters */ 00449 inline double getArea() const { return size_x*size_y*square(resolution); } 00450 00451 /** Returns the horizontal size of grid map in cells count. 00452 */ 00453 inline unsigned int getSizeX() const { return size_x; } 00454 00455 /** Returns the vertical size of grid map in cells count. 00456 */ 00457 inline unsigned int getSizeY() const { return size_y; } 00458 00459 /** Returns the "x" coordinate of left side of grid map. 00460 */ 00461 inline float getXMin() const { return x_min; } 00462 00463 /** Returns the "x" coordinate of right side of grid map. 00464 */ 00465 inline float getXMax() const { return x_max; } 00466 00467 /** Returns the "y" coordinate of top side of grid map. 00468 */ 00469 inline float getYMin() const { return y_min; } 00470 00471 /** Returns the "y" coordinate of bottom side of grid map. 00472 */ 00473 inline float getYMax() const { return y_max; } 00474 00475 /** Returns the resolution of the grid map. 00476 */ 00477 inline float getResolution() const { return resolution; } 00478 00479 /** Transform a coordinate value into a cell index. 00480 */ 00481 inline int x2idx(float x) const { return static_cast<int>((x-x_min)/resolution ); } 00482 inline int y2idx(float y) const { return static_cast<int>((y-y_min)/resolution ); } 00483 00484 inline int x2idx(double x) const { return static_cast<int>((x-x_min)/resolution ); } 00485 inline int y2idx(double y) const { return static_cast<int>((y-y_min)/resolution ); } 00486 00487 /** Transform a cell index into a coordinate value. 00488 */ 00489 inline float idx2x(const size_t cx) const { return x_min+(cx+0.5f)*resolution; } 00490 inline float idx2y(const size_t cy) const { return y_min+(cy+0.5f)*resolution; } 00491 00492 /** Transform a coordinate value into a cell index, using a diferent "x_min" value 00493 */ 00494 inline int x2idx(float x,float x_min) const { return static_cast<int>((x-x_min)/resolution ); } 00495 inline int y2idx(float y, float y_min) const { return static_cast<int>((y-y_min)/resolution ); } 00496 00497 /** Scales an integer representation of the log-odd into a real valued probability in [0,1], using p=exp(l)/(1+exp(l)) 00498 */ 00499 static inline float l2p(const cellType &l) 00500 { 00501 return logoddsTablePtr[ -OCCGRID_CELLTYPE_MIN+l ]; 00502 } 00503 00504 /** Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)) 00505 */ 00506 static inline uint8_t l2p_255(const cellType &l) 00507 { 00508 return logoddsTable_255Ptr[ -OCCGRID_CELLTYPE_MIN+l ]; 00509 } 00510 00511 /** Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the valid range of cellType. 00512 */ 00513 static inline cellType p2l(const float &p) 00514 { 00515 return p2lTablePtr[ (int)(p * OCCGRID_P2LTABLE_SIZE) ]; 00516 } 00517 00518 /** Change the contents [0,1] of a cell, given its index. 00519 */ 00520 inline void setCell(int x,int y,float value) 00521 { 00522 // The x> comparison implicitly holds if x<0 00523 if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y) 00524 return; 00525 else map[x+y*size_x]=p2l(value); 00526 } 00527 00528 /** Read the real valued [0,1] contents of a cell, given its index. 00529 */ 00530 inline float getCell(int x,int y) const 00531 { 00532 // The x> comparison implicitly holds if x<0 00533 if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y) 00534 return 0.5f; 00535 else return l2p(map[x+y*size_x]); 00536 } 00537 00538 /** Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally. 00539 */ 00540 inline cellType *getRow( int cy ) { if (cy<0 || static_cast<unsigned int>(cy)>=size_y) return NULL; else return &map[0+cy*size_x]; } 00541 00542 /** Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally. 00543 */ 00544 inline const cellType *getRow( int cy ) const { if (cy<0 || static_cast<unsigned int>(cy)>=size_y) return NULL; else return &map[0+cy*size_x]; } 00545 00546 /** Change the contents [0,1] of a cell, given its coordinates. 00547 */ 00548 inline void setPos(float x,float y,float value) { setCell(x2idx(x),y2idx(y),value); } 00549 00550 /** Read the real valued [0,1] contents of a cell, given its coordinates. 00551 */ 00552 inline float getPos(float x,float y) const { return getCell(x2idx(x),y2idx(y)); } 00553 00554 /** Returns "true" if cell is "static", i.e.if its occupancy is below a given threshold. 00555 */ 00556 inline bool isStaticPos(float x,float y,float threshold = 0.7f) const { return isStaticCell(x2idx(x),y2idx(y),threshold); } 00557 inline bool isStaticCell(int cx,int cy,float threshold = 0.7f) const { return (getCell(cx,cy)<=threshold); } 00558 00559 /** Change a cell in the "basis" maps.Used for Voronoi calculation. 00560 */ 00561 inline void setBasisCell(int x,int y,unsigned char value) { basis_map[x+y*size_x]=value; }; 00562 00563 /** Reads a cell in the "basis" maps.Used for Voronoi calculation. 00564 */ 00565 inline unsigned char getBasisCell(int x,int y) const { return basis_map[x+y*size_x]; }; 00566 00567 /** Used for returning entropy related information 00568 * \sa computeEntropy 00569 */ 00570 struct MAPS_IMPEXP TEntropyInfo 00571 { 00572 TEntropyInfo() : H(0),I(0),mean_H(0),mean_I(0),effectiveMappedArea(0),effectiveMappedCells(0) 00573 { 00574 } 00575 00576 /** The target variable for absolute entropy, computed as:<br><center>H(map)=Sum<sub>x,y</sub>{ -p(x,y)·ln(p(x,y)) -(1-p(x,y))·ln(1-p(x,y)) }</center><br><br> 00577 */ 00578 double H; 00579 00580 /** The target variable for absolute "information", defining I(x) = 1 - H(x) 00581 */ 00582 double I; 00583 00584 /** The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells) 00585 */ 00586 double mean_H; 00587 00588 /** The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (cells) 00589 */ 00590 double mean_I; 00591 00592 /** The target variable for the area of cells with information, i.e. p(x)!=0.5 00593 */ 00594 double effectiveMappedArea; 00595 00596 /** The mapped area in cells. 00597 */ 00598 unsigned long effectiveMappedCells; 00599 }; 00600 00601 /** With this struct options are provided to the observation insertion process. 00602 * \sa CObservation::insertIntoGridMap 00603 */ 00604 class MAPS_IMPEXP TInsertionOptions : public mrpt::utils::CLoadableOptions 00605 { 00606 public: 00607 /** Initilization of default parameters 00608 */ 00609 TInsertionOptions( ); 00610 00611 /** This method load the options from a ".ini" file. 00612 * Only those parameters found in the given "section" and having 00613 * the same name that the variable are loaded. Those not found in 00614 * the file will stay with their previous values (usually the default 00615 * values loaded at initialization). An example of an ".ini" file: 00616 * \code 00617 * [section] 00618 * resolution=0.10 ; blah blah... 00619 * modeSelection=1 ; 0=blah, 1=blah,... 00620 * \endcode 00621 */ 00622 void loadFromConfigFile( 00623 const mrpt::utils::CConfigFileBase &source, 00624 const std::string §ion); 00625 00626 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. 00627 */ 00628 void dumpToTextStream(CStream &out) const; 00629 00630 00631 /** The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map! 00632 */ 00633 float mapAltitude; 00634 00635 /** The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true. 00636 */ 00637 bool useMapAltitude; 00638 00639 /** The largest distance at which cells will be updated (Default 15 meters) 00640 */ 00641 float maxDistanceInsertion; 00642 00643 /** A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0.8) 00644 */ 00645 float maxOccupancyUpdateCertainty; 00646 00647 /** If set to true (default), invalid range values (no echo rays) as consider as free space until "maxOccupancyUpdateCertainty", but ONLY when the previous and next rays are also an invalid ray. 00648 */ 00649 bool considerInvalidRangesAsFreeSpace; 00650 00651 /** Specify the decimation of the range scan (default=1 : take all the range values!) 00652 */ 00653 uint16_t decimation; 00654 00655 /** The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, then processed by calls to this class (default=0). */ 00656 float horizontalTolerance; 00657 00658 /** Gaussian sigma of the filter used in getAsImageFiltered (for features detection) (Default=1) (0:Disabled) */ 00659 float CFD_features_gaussian_size; 00660 00661 /** Size of the Median filter used in getAsImageFiltered (for features detection) (Default=3) (0:Disabled) */ 00662 float CFD_features_median_size; 00663 00664 bool wideningBeamsWithDistance; //!< Enabled: Rays widen with distance to approximate the real behavior of lasers, disabled: insert rays as simple lines (Default=true) 00665 00666 }; 00667 00668 /** With this struct options are provided to the observation insertion process. 00669 * \sa CObservation::insertIntoGridMap 00670 */ 00671 TInsertionOptions insertionOptions; 00672 00673 /** The type for selecting a likelihood computation method 00674 */ 00675 enum TLikelihoodMethod 00676 { 00677 lmMeanInformation = 0, 00678 lmRayTracing, 00679 lmConsensus, 00680 lmCellsDifference, 00681 lmLikelihoodField_Thrun, 00682 lmLikelihoodField_II, 00683 lmConsensusOWA 00684 }; 00685 00686 /** With this struct options are provided to the observation likelihood computation process. 00687 */ 00688 class MAPS_IMPEXP TLikelihoodOptions : public mrpt::utils::CLoadableOptions 00689 { 00690 public: 00691 /** Initilization of default parameters 00692 */ 00693 TLikelihoodOptions(); 00694 00695 /** This method load the options from a ".ini" file. 00696 * Only those parameters found in the given "section" and having 00697 * the same name that the variable are loaded. Those not found in 00698 * the file will stay with their previous values (usually the default 00699 * values loaded at initialization). An example of an ".ini" file: 00700 * \code 00701 * [section] 00702 * resolution=0.10 ; blah blah... 00703 * modeSelection=1 ; 0=blah, 1=blah,... 00704 * \endcode 00705 */ 00706 void loadFromConfigFile( 00707 const mrpt::utils::CConfigFileBase &source, 00708 const std::string §ion); 00709 00710 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. 00711 */ 00712 void dumpToTextStream(CStream &out) const; 00713 00714 /** The selected method to compute an observation likelihood. 00715 */ 00716 TLikelihoodMethod likelihoodMethod; 00717 00718 /** [LikelihoodField] The laser range "sigma" used in computations; Default value = 0.35 00719 */ 00720 float LF_stdHit; 00721 00722 /** [LikelihoodField] Ratios of the hit/random components of the likelihood; Default values=0.95/0.5 00723 */ 00724 float LF_zHit, LF_zRandom; 00725 00726 /** [LikelihoodField] The max. range of the sensor (def=81meters) 00727 */ 00728 float LF_maxRange; 00729 00730 /** [LikelihoodField] The decimation of the points in a scan, default=1 == no decimation. 00731 */ 00732 uint32_t LF_decimation; 00733 00734 /** [LikelihoodField] The max. distance for searching correspondences around each sensed point 00735 */ 00736 float LF_maxCorrsDistance; 00737 00738 /** [LikelihoodField] Set this to "true" ot use an alternative method, where the likelihood of the whole range scan is computed by "averaging" of individual ranges, instead of by the "product". 00739 */ 00740 bool LF_alternateAverageMethod; 00741 00742 /** [MI] The exponent in the MI likelihood computation. Default value = 5 00743 */ 00744 float MI_exponent; 00745 00746 /** [MI] The scan rays decimation: at every N rays, one will be used to compute the MI: 00747 */ 00748 uint32_t MI_skip_rays; 00749 00750 /** [MI] The ratio for the max. distance used in the MI computation and in the insertion of scans, e.g. if set to 2.0 the MI will use twice the distance that the update distance. 00751 */ 00752 float MI_ratio_max_distance; 00753 00754 /** [rayTracing] If true (default), the rayTracing method will ignore measured ranges shorter than the simulated ones. 00755 */ 00756 bool rayTracing_useDistanceFilter; 00757 00758 /** [rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to use all the sensed ranges. 00759 */ 00760 int32_t rayTracing_decimation; 00761 00762 /** [rayTracing] The laser range sigma. 00763 */ 00764 float rayTracing_stdHit; 00765 00766 /** [Consensus] The down-sample ratio of ranges (default=1, consider all the ranges) 00767 */ 00768 int32_t consensus_takeEachRange; 00769 00770 /** [Consensus] The power factor for the likelihood (default=5) 00771 */ 00772 float consensus_pow; 00773 00774 /** [OWA] The sequence of weights to be multiplied to of the ordered list of likelihood values (first one is the largest); the size of this vector determines the number of highest likelihood values to fuse. 00775 */ 00776 std::vector<float> OWA_weights; 00777 00778 /** Enables the usage of a cache of likelihood values (for LF methods), if set to true (default=false). 00779 */ 00780 bool enableLikelihoodCache; 00781 00782 } likelihoodOptions; 00783 00784 /** Auxiliary private class. 00785 */ 00786 typedef std::pair<double,CPoint2D> TPairLikelihoodIndex; 00787 00788 /** Some members of this struct will contain intermediate or output data after calling "computeObservationLikelihood" for some likelihood functions. 00789 */ 00790 class TLikelihoodOutput 00791 { 00792 public: 00793 TLikelihoodOutput() : OWA_pairList(), OWA_individualLikValues() 00794 {} 00795 00796 /** [OWA method] This will contain the ascending-ordered list of pairs:(likelihood values, 2D point in map coordinates). 00797 */ 00798 std::vector<TPairLikelihoodIndex> OWA_pairList; 00799 00800 /** [OWA method] This will contain the ascending-ordered list of likelihood values for individual range measurements in the scan. 00801 */ 00802 std::vector<double> OWA_individualLikValues; 00803 00804 } likelihoodOutputs; 00805 00806 /** Performs a downsampling of the gridmap, by a given factor: resolution/=ratio 00807 */ 00808 void subSample( int downRatio ); 00809 00810 /** Computes the entropy and related values of this grid map. 00811 * The entropy is computed as the summed entropy of each cell, taking them as discrete random variables following a Bernoulli distribution: 00812 * \param info The output information is returned here. 00813 */ 00814 void computeEntropy( TEntropyInfo &info ) const; 00815 00816 /** Reads a the clearance of a cell, after building the Voronoi diagram. 00817 */ 00818 int getVoroniClearance(int cx,int cy) { return voronoi_diagram[cx+cy*size_x]; } 00819 00820 /** Used to set the clearance of a cell, while building the Voronoi diagram. 00821 */ 00822 void setVoroniClearance(int cx,int cy,int dist) { voronoi_diagram[cx+cy*size_x]=dist; } 00823 00824 /** Build the Voronoi diagram of the grid map. 00825 * \param threshold The threshold for binarizing the map. 00826 * \param robot_size Size in "units" (meters) of robot, approx. 00827 * \param x1 Left coordinate of area to be computed. Default, entire map. 00828 * \param x2 Right coordinate of area to be computed. Default, entire map. 00829 * \param y1 Top coordinate of area to be computed. Default, entire map. 00830 * \param y2 Bottom coordinate of area to be computed. Default, entire map. 00831 * \sa FindCriticalPoints 00832 */ 00833 void buildVoronoiDiagram(float threshold, float robot_size,int x1=0,int x2=0, int y1=0,int y2=0); 00834 00835 /** Builds a list with the critical points from Voronoi diagram, which must 00836 * must be built before calling this method. 00837 * \param filter_distance The minimum distance between two critical points. 00838 * \sa Build_VoronoiDiagram 00839 */ 00840 void findCriticalPoints( float filter_distance ); 00841 00842 /** Compute the clearance of a given cell, and returns its two first 00843 * basis (closest obstacle) points.Used to build Voronoi and critical points. 00844 * \return The clearance of the cell, in 1/100 of "cell". 00845 * \param cx The cell index 00846 * \param cy The cell index 00847 * \param basis_x Target buffer for coordinates of basis, having a size of two "ints". 00848 * \param basis_y Target buffer for coordinates of basis, having a size of two "ints". 00849 * \param nBasis The number of found basis: Can be 0,1 or 2. 00850 * \param GetContourPoint If "true" the basis are not returned, but the closest free cells.Default at false. 00851 * \sa Build_VoronoiDiagram 00852 */ 00853 int computeClearance( int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint = false ) const; 00854 00855 /** An alternative method for computing the clearance of a given location (in meters). 00856 * \return The clearance (distance to closest OCCUPIED cell), in meters. 00857 */ 00858 float computeClearance( float x, float y, float maxSearchDistance ) const; 00859 00860 /** Compute the 'cost' of traversing a segment of the map according to the occupancy of traversed cells. 00861 * \return This returns '1-mean(traversed cells occupancy)', i.e. 0.5 for unknown cells, 1 for a free path. 00862 */ 00863 float computePathCost( float x1, float y1, float x2, float y2 ) const; 00864 00865 00866 /** \name Sensor simulators 00867 @{ 00868 */ 00869 00870 /** Simulates a laser range scan into the current grid map. 00871 * The simulated scan is stored in a CObservation2DRangeScan object, which is also used 00872 * to pass some parameters: all previously stored characteristics (as aperture,...) are 00873 * taken into account for simulation. Only a few more parameters are needed. Additive gaussian noise can be optionally added to the simulated scan. 00874 * \param inout_Scan [IN/OUT] This must be filled with desired parameters before calling, and will contain the scan samples on return. 00875 * \param robotPose [IN] The robot pose in this map coordinates. Recall that sensor pose relative to this robot pose must be specified in the observation object. 00876 * \param threshold [IN] The minimum occupancy threshold to consider a cell to be occupied, for example 0.5. 00877 * \param N [IN] The count of range scan "rays", by default to 361. 00878 * \param noiseStd [IN] The standard deviation of measurement noise. If not desired, set to 0. 00879 * \param decimation [IN] The rays that will be simulated are at indexes: 0, D, 2D, 3D, ... Default is D=1 00880 * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians). 00881 * 00882 * \sa sonarSimulator 00883 */ 00884 void laserScanSimulator( 00885 CObservation2DRangeScan &inout_Scan, 00886 const CPose2D &robotPose, 00887 float threshold = 0.5f, 00888 size_t N = 361, 00889 float noiseStd = 0, 00890 unsigned int decimation = 1, 00891 float angleNoiseStd = DEG2RAD(0) ) const; 00892 00893 /** Simulates the observations of a sonar rig into the current grid map. 00894 * The simulated ranges are stored in a CObservationRange object, which is also used 00895 * to pass in some needed parameters, as the poses of the sonar sensors onto the mobile robot. 00896 * \param inout_observation [IN/OUT] This must be filled with desired parameters before calling, and will contain the simulated ranges on return. 00897 * \param robotPose [IN] The robot pose in this map coordinates. Recall that sensor pose relative to this robot pose must be specified in the observation object. 00898 * \param threshold [IN] The minimum occupancy threshold to consider a cell to be occupied, for example 0.5. 00899 * \param rangeNoiseStd [IN] The standard deviation of measurement noise. If not desired, set to 0. 00900 * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians). 00901 * 00902 * \sa laserScanSimulator 00903 */ 00904 void sonarSimulator( 00905 CObservationRange &inout_observation, 00906 const CPose2D &robotPose, 00907 float threshold = 0.5f, 00908 float rangeNoiseStd = 0, 00909 float angleNoiseStd = DEG2RAD(0) ) const; 00910 00911 /** Simulate just one "ray" in the grid map. This method is used internally to sonarSimulator and laserScanSimulator. 00912 */ 00913 inline void simulateScanRay( 00914 const double x,const double y,const double angle_direction, 00915 float &out_range,bool &out_valid, 00916 const unsigned int max_ray_len, 00917 const float threshold_free=0.5f, 00918 const double noiseStd=0, const double angleNoiseStd=0 ) const; 00919 00920 00921 /** @} */ 00922 00923 /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map. 00924 * See "likelihoodOptions" for configuration parameters. 00925 * 00926 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00927 * \param obs The observation. 00928 * \return This method returns a likelihood in the range [0,1]. 00929 * 00930 * Used in particle filter algorithms, see: CMultiMetricMapPDF::prediction_and_update 00931 * 00932 * \sa likelihoodOptions, likelihoodOutputs 00933 */ 00934 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 00935 00936 /** Returns true if this map is able to compute a sensible likelihood function for this observation (i.e. an occupancy grid map cannot with an image). 00937 * \param obs The observation. 00938 * \sa computeObservationLikelihood 00939 */ 00940 bool canComputeObservationLikelihood( const CObservation *obs ); 00941 00942 /** Computes the likelihood [0,1] of a set of points, given the current grid map as reference. 00943 * \param pm The points map 00944 * \param relativePose The relative pose of the points map in this map's coordinates, or NULL for (0,0,0). 00945 * See "likelihoodOptions" for configuration parameters. 00946 */ 00947 double computeLikelihoodField_Thrun( const CPointsMap *pm, const CPose2D *relativePose = NULL); 00948 00949 /** Computes the likelihood [0,1] of a set of points, given the current grid map as reference. 00950 * \param pm The points map 00951 * \param relativePose The relative pose of the points map in this map's coordinates, or NULL for (0,0,0). 00952 * See "likelihoodOptions" for configuration parameters. 00953 */ 00954 double computeLikelihoodField_II( const CPointsMap *pm, const CPose2D *relativePose = NULL); 00955 00956 /** Saves the gridmap as a graphical file (BMP,PNG,...). 00957 * The format will be derived from the file extension (see CImage::saveToFile ) 00958 * \return False on any error. 00959 */ 00960 bool saveAsBitmapFile(const std::string &file) const; 00961 00962 /** Saves a composite image with two gridmaps and lines representing a set of correspondences between them. 00963 * \sa saveAsEMFTwoMapsWithCorrespondences 00964 * \return False on any error. 00965 */ 00966 static bool saveAsBitmapTwoMapsWithCorrespondences( 00967 const std::string &fileName, 00968 const COccupancyGridMap2D *m1, 00969 const COccupancyGridMap2D *m2, 00970 const TMatchingPairList &corrs); 00971 00972 /** Saves a composite image with two gridmaps and numbers for the correspondences between them. 00973 * \sa saveAsBitmapTwoMapsWithCorrespondences 00974 * \return False on any error. 00975 */ 00976 static bool saveAsEMFTwoMapsWithCorrespondences( 00977 const std::string &fileName, 00978 const COccupancyGridMap2D *m1, 00979 const COccupancyGridMap2D *m2, 00980 const TMatchingPairList &corrs); 00981 00982 /** Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel is 1 cell, and with an overlay of landmarks. 00983 * \note The template parameter CLANDMARKSMAP is assumed to be mrpt::slam::CLandmarksMap normally. 00984 * \return False on any error. 00985 */ 00986 template <class CLANDMARKSMAP> 00987 bool saveAsBitmapFileWithLandmarks( 00988 const std::string &file, 00989 const CLANDMARKSMAP *landmarks, 00990 bool addTextLabels = false, 00991 const mrpt::utils::TColor &marks_color = mrpt::utils::TColor(0,0,255) ) const 00992 { 00993 MRPT_START 00994 CImage img(1,1,3); 00995 getAsImageFiltered( img, false, true ); // in RGB 00996 const bool topleft = img.isOriginTopLeft(); 00997 for (unsigned int i=0;i<landmarks->landmarks.size();i++) 00998 { 00999 const typename CLANDMARKSMAP::landmark_type *lm = landmarks->landmarks.get( i ); 01000 int px = x2idx( lm->pose_mean.x ); 01001 int py = topleft ? size_y-1- y2idx( lm->pose_mean.y ) : y2idx( lm->pose_mean.y ); 01002 img.rectangle( px - 7, (py + 7), px +7, (py -7), marks_color ); 01003 img.rectangle( px - 6, (py + 6), px +6, (py -6), marks_color ); 01004 if (addTextLabels) 01005 img.textOut(px,py-8,format("%u",i), TColor::black); 01006 } 01007 return img.saveToFile(file.c_str() ); 01008 MRPT_END 01009 } 01010 01011 01012 /** Returns the grid as a float image, where each pixel is a cell 01013 */ 01014 void getAsImage( utils::CImageFloat &img, bool verticalFlip = false ) const; 01015 01016 /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true) 01017 * If "tricolor" is true, only three gray levels will appear in the image: gray for unobserved cells, and black/white for occupied/empty cells respectively. 01018 * \sa getAsImageFiltered 01019 */ 01020 void getAsImage( utils::CImage &img, bool verticalFlip = false, bool forceRGB=false, bool tricolor = false) const; 01021 01022 /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true) - This method filters the image for easy feature detection 01023 * If "tricolor" is true, only three gray levels will appear in the image: gray for unobserved cells, and black/white for occupied/empty cells respectively. 01024 * \sa getAsImage 01025 */ 01026 void getAsImageFiltered( utils::CImage &img, bool verticalFlip = false, bool forceRGB=false) const; 01027 01028 /** Returns a 3D plane with its texture being the occupancy grid and transparency proportional to "uncertainty" (i.e. a value of 0.5 is fully transparent) 01029 */ 01030 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 01031 01032 /** Returns true upon map construction or after calling clear(), the return 01033 * changes to false upon successful insertObservation() or any other method to load data in the map. 01034 */ 01035 bool isEmpty() const; 01036 01037 /** Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile). 01038 * \param file The file to be loaded. 01039 * \param resolution The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed. 01040 * \param xCentralPixel The "x" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map. 01041 * \param yCentralPixel The "y" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map. 01042 * \return False on any error. 01043 * \sa loadFromBitmap 01044 */ 01045 bool loadFromBitmapFile(const std::string &file, float resolution, float xCentralPixel = -1, float yCentralPixel =-1 ); 01046 01047 /** Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile). 01048 * \param img The image. Only a grayscale image will be used, so RGB components will be mixed if a color image is passed. 01049 * \param resolution The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed. 01050 * \param xCentralPixel The "x" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map. 01051 * \param yCentralPixel The "y" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map. 01052 * \return False on any error. 01053 * \sa loadFromBitmapFile 01054 */ 01055 bool loadFromBitmap(const mrpt::utils::CImage &img, float resolution, float xCentralPixel = -1, float yCentralPixel =-1 ); 01056 01057 /** See the base class for more details: In this class it is implemented as correspondences of the passed points map to occupied cells. 01058 * NOTICE: That the "z" dimension is ignored in the points. Clip the points as appropiated if needed before calling this method. 01059 * 01060 * \sa computeMatching3DWith 01061 */ 01062 void computeMatchingWith2D( 01063 const CMetricMap *otherMap, 01064 const CPose2D &otherMapPose, 01065 float maxDistForCorrespondence, 01066 float maxAngularDistForCorrespondence, 01067 const CPose2D &angularDistPivotPoint, 01068 TMatchingPairList &correspondences, 01069 float &correspondencesRatio, 01070 float *sumSqrDist = NULL, 01071 bool onlyKeepTheClosest = false, 01072 bool onlyUniqueRobust = false ) const; 01073 01074 01075 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 01076 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 01077 * \param otherMap [IN] The other map to compute the matching with. 01078 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 01079 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 01080 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 01081 * 01082 * \return The matching ratio [0,1] 01083 * \sa computeMatchingWith2D 01084 */ 01085 float compute3DMatchingRatio( 01086 const CMetricMap *otherMap, 01087 const CPose3D &otherMapPose, 01088 float minDistForCorr = 0.10f, 01089 float minMahaDistForCorr = 2.0f 01090 ) const; 01091 01092 /** This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface). 01093 */ 01094 void saveMetricMapRepresentationToFile( 01095 const std::string &filNamePrefix 01096 ) const; 01097 01098 /** The structure used to store the set of Voronoi diagram 01099 * critical points. 01100 * \sa FindCriticalPoints 01101 */ 01102 struct MAPS_IMPEXP TCriticalPointsList 01103 { 01104 TCriticalPointsList() : x(),y(),clearance(),x_basis1(),y_basis1(),x_basis2(),y_basis2() 01105 {} 01106 01107 /** The coordinates of critical point. 01108 */ 01109 std::vector<int> x,y; 01110 /** The clearance of critical points, in 1/100 of cells. 01111 */ 01112 std::vector<int> clearance; 01113 /** Their two first basis points coordinates. 01114 */ 01115 std::vector<int> x_basis1,y_basis1, x_basis2,y_basis2; 01116 } CriticalPointsList; 01117 01118 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 01119 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 01120 */ 01121 void auxParticleFilterCleanUp(); 01122 01123 01124 private: 01125 /** Returns a byte with the occupancy of the 8 sorrounding cells. 01126 * \param cx The cell index 01127 * \param cy The cell index 01128 * \sa direction2idx 01129 */ 01130 inline unsigned char GetNeighborhood( int cx, int cy ) const; 01131 01132 /** Used to store the 8 possible movements from a cell to the 01133 * sorrounding ones.Filled in the constructor. 01134 * \sa direction2idx 01135 */ 01136 int direccion_vecino_x[8],direccion_vecino_y[8]; 01137 01138 /** Returns the index [0,7] of the given movement, or 01139 * -1 if invalid one. 01140 * \sa direccion_vecino_x,direccion_vecino_y,GetNeighborhood 01141 */ 01142 int direction2idx(int dx, int dy); 01143 }; 01144 01145 01146 bool operator <(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2); 01147 01148 } // End of namespace 01149 } // End of namespace 01150 01151 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
