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