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