Main MRPT website > C++ reference
MRPT logo

COccupancyGridMap2D.h

Go to the documentation of this file.
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 &section);
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 &section);
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: at Sat Mar 26 06:40:17 UTC 2011