Main MRPT website > C++ reference
MRPT logo
COccupancyGridMap2D.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 
29 #ifndef COccupancyGridMap2D_H
30 #define COccupancyGridMap2D_H
31 
32 #include <mrpt/utils/CSerializable.h>
33 #include <mrpt/utils/CLoadableOptions.h>
34 #include <mrpt/utils/CImage.h>
35 #include <mrpt/utils/CDynamicGrid.h>
36 #include <mrpt/slam/CMetricMap.h>
37 #include <mrpt/utils/TMatchingPair.h>
38 #include <mrpt/slam/CLogOddsGridMap2D.h>
39 
40 #include <mrpt/maps/link_pragmas.h>
41 
42 #include <mrpt/utils/safe_pointers.h>
43 
44 #include <mrpt/config.h>
45 
46 #if !defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && !defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS)
47  #error One of OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS or OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS must be defined.
48 #endif
49 
50 #if defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS)
51  #error Only one of OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS or OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS must be defined at a time.
52 #endif
53 
54 
55 namespace mrpt
56 {
57  namespace poses { class CPose2D; }
58 namespace slam
59 {
60  using namespace mrpt::poses;
61  using namespace mrpt::utils;
62 
63  class CObservation2DRangeScan;
64  class CObservationRange;
65  class CObservation;
66  class CPointsMap;
67 
68  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( COccupancyGridMap2D, CMetricMap, MAPS_IMPEXP )
69 
70  /** A class for storing an occupancy grid map.
71  * COccupancyGridMap2D is a class for storing a metric map
72  * representation in the form of a probabilistic occupancy
73  * grid map: value of 0 means certainly occupied, 1 means
74  * a certainly empty cell. Initially 0.5 means uncertainty.
75  *
76  * The cells keep the log-odd representation of probabilities instead of the probabilities themselves.
77  * More details can be found at http://www.mrpt.org/Occupancy_Grids
78  *
79  * The algorithm for updating the grid from a laser scanner can optionally take into account the progressive widening of the beams, as
80  * described in the <a href="http://www.mrpt.org/Occupancy_Grids" > wiki </a> (this feature was introduced in MRPT 0.6.4).
81  *
82  * Some implemented methods are:
83  * - Update of individual cells
84  * - Insertion of observations
85  * - Voronoi diagram and critical points (\a buildVoronoiDiagram)
86  * - Saving and loading from/to a bitmap
87  * - Laser scans simulation for the map contents
88  * - Entropy and information methods (See computeEntropy)
89  *
90  * \ingroup mrpt_maps_grp
91  **/
92  class MAPS_IMPEXP COccupancyGridMap2D :
93  public CMetricMap,
94  // Inherit from the corresponding specialization of CLogOddsGridMap2D<>:
95 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
96  public CLogOddsGridMap2D<int8_t>
97 #else
98  public CLogOddsGridMap2D<int16_t>
99 #endif
100  {
101  // This must be added to any CSerializable derived class:
102  DEFINE_SERIALIZABLE( COccupancyGridMap2D )
103 
104  public:
105  /** The type of the map cells: */
106 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
107  typedef int8_t cellType;
108  typedef uint8_t cellTypeUnsigned;
109 #else
110  typedef int16_t cellType;
111  typedef uint16_t cellTypeUnsigned;
112 #endif
113 
114  /** Discrete to float conversion factors: The min/max values of the integer cell type, eg.[0,255] or [0,65535] */
115  static const cellType OCCGRID_CELLTYPE_MIN = CLogOddsGridMap2D<cellType>::CELLTYPE_MIN;
116  static const cellType OCCGRID_CELLTYPE_MAX = CLogOddsGridMap2D<cellType>::CELLTYPE_MAX;
117  static const cellType OCCGRID_P2LTABLE_SIZE = CLogOddsGridMap2D<cellType>::P2LTABLE_SIZE;
118 
119  protected:
120 
121  friend class CMultiMetricMap;
122  friend class CMultiMetricMapPDF;
123 
124  static CLogOddsGridMapLUT<cellType> m_logodd_lut; //!< Lookup tables for log-odds
125 
126  /** This is the buffer for storing the cells.In this dynamic
127  * size buffer are stored the cell values as
128  * "bytes", stored row by row, from left to right cells.
129  */
130  std::vector<cellType> map;
131 
132  /** The size of the grid in cells.
133  */
134  uint32_t size_x,size_y;
135 
136  /** The limits of the grid in "units" (meters).
137  */
138  float x_min,x_max,y_min,y_max;
139 
140  /** Cell size, i.e. resolution of the grid map.
141  */
142  float resolution;
143 
144  /** 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).
145  */
146  std::vector<double> precomputedLikelihood;
147  bool precomputedLikelihoodToBeRecomputed;
148 
149  /** Used for Voronoi calculation.Same struct as "map", but contains a "0" if not a basis point. */
150  CDynamicGrid<uint8_t> m_basis_map;
151 
152  /** Used to store the Voronoi diagram.
153  * Contains the distance of each cell to its closer obstacles
154  * in 1/100th distance units (i.e. in centimeters), or 0 if not into the Voronoi diagram.
155  */
156  CDynamicGrid<uint16_t> m_voronoi_diagram;
157 
158  bool m_is_empty; //!< True upon construction; used by isEmpty()
159 
160  virtual void OnPostSuccesfulInsertObs(const CObservation *); //!< See base class
161 
162  /** The free-cells threshold used to compute the Voronoi diagram.
163  */
164  float voroni_free_threshold;
165 
166  /** Frees the dynamic memory buffers of map.
167  */
168  void freeMap();
169 
170  /** Entropy computation internal function:
171  */
172  static double H(double p);
173 
174  /** Change the contents [0,1] of a cell, given its index.
175  */
176  inline void setCell_nocheck(int x,int y,float value)
177  {
178  map[x+y*size_x]=p2l(value);
179  }
180 
181  /** Read the real valued [0,1] contents of a cell, given its index.
182  */
183  inline float getCell_nocheck(int x,int y) const
184  {
185  return l2p(map[x+y*size_x]);
186  }
187 
188  /** Changes a cell by its absolute index (Do not use it normally)
189  */
190  inline void setRawCell(unsigned int cellIndex, cellType b)
191  {
192  if (cellIndex<size_x*size_y)
193  {
194  map[cellIndex] = b;
195  }
196  }
197 
198  /** Internally used to speed-up entropy calculation
199  */
200  static std::vector<float> entropyTable;
201 
202 
203  /** 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.)
204  */
205  double computeObservationLikelihood_Consensus(
206  const CObservation *obs,
207  const CPose2D &takenFrom );
208 
209  /** One of the methods that can be selected for implementing "computeObservationLikelihood"
210  * TODO: This method is described in....
211  */
212  double computeObservationLikelihood_ConsensusOWA(
213  const CObservation *obs,
214  const CPose2D &takenFrom );
215 
216  /** One of the methods that can be selected for implementing "computeObservationLikelihood".
217  */
218  double computeObservationLikelihood_CellsDifference(
219  const CObservation *obs,
220  const CPose2D &takenFrom );
221 
222  /** One of the methods that can be selected for implementing "computeObservationLikelihood".
223  */
224  double computeObservationLikelihood_MI(
225  const CObservation *obs,
226  const CPose2D &takenFrom );
227 
228  /** One of the methods that can be selected for implementing "computeObservationLikelihood".
229  */
230  double computeObservationLikelihood_rayTracing(
231  const CObservation *obs,
232  const CPose2D &takenFrom );
233 
234  /** One of the methods that can be selected for implementing "computeObservationLikelihood".
235  */
236  double computeObservationLikelihood_likelihoodField_Thrun(
237  const CObservation *obs,
238  const CPose2D &takenFrom );
239 
240  /** One of the methods that can be selected for implementing "computeObservationLikelihood".
241  */
242  double computeObservationLikelihood_likelihoodField_II(
243  const CObservation *obs,
244  const CPose2D &takenFrom );
245 
246  /** 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).
247  */
248  virtual void internal_clear( );
249 
250  /** Insert the observation information into this map.
251  *
252  * \param obs The observation
253  * \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)
254  *
255  * After successfull execution, "lastObservationInsertionInfo" is updated.
256  *
257  * \sa insertionOptions, CObservation::insertObservationInto
258  */
259  virtual bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
260 
261  public:
262 
263  /** Performs the Bayesian fusion of a new observation of a cell.
264  * \sa updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free
265  */
266  void updateCell(int x,int y, float v);
267 
268  /** An internal structure for storing data related to counting the new information apported by some observation.
269  */
270  struct MAPS_IMPEXP TUpdateCellsInfoChangeOnly
271  {
272  TUpdateCellsInfoChangeOnly( bool enabled = false,
273  double I_change = 0,
274  int cellsUpdated=0) : enabled(enabled),
275  I_change(I_change),
276  cellsUpdated(cellsUpdated),
277  laserRaysSkip(1)
278  {
279  }
280 
281  /** If set to false (default), this struct is not used. Set to true only when measuring the info of an observation.
282  */
283  bool enabled;
284 
285  /** The cummulative change in Information: This is updated only from the "updateCell" method.
286  */
287  double I_change;
288 
289  /** The cummulative updated cells count: This is updated only from the "updateCell" method.
290  */
291  int cellsUpdated;
292 
293  /** In this mode, some laser rays can be skips to speep-up
294  */
295  int laserRaysSkip;
296  } updateInfoChangeOnly;
297 
298  /** Constructor.
299  */
300  COccupancyGridMap2D( float min_x = -20.0f,
301  float max_x = 20.0f,
302  float min_y = -20.0f,
303  float max_y = 20.0f,
304  float resolution = 0.05f
305  );
306 
307  /** Fills all the cells with a default value.
308  */
309  void fill(float default_value = 0.5f );
310 
311  /** Destructor.
312  */
313  virtual ~COccupancyGridMap2D();
314 
315  /** Change the size of gridmap, erasing all its previous contents.
316  * \param x_min The "x" coordinates of left most side of grid.
317  * \param x_max The "x" coordinates of right most side of grid.
318  * \param y_min The "y" coordinates of top most side of grid.
319  * \param y_max The "y" coordinates of bottom most side of grid.
320  * \param resolution The new size of cells.
321  * \param default_value The value of cells, tipically 0.5.
322  * \sa ResizeGrid
323  */
324  void setSize(float x_min,float x_max,float y_min,float y_max,float resolution,float default_value = 0.5f);
325 
326  /** Change the size of gridmap, maintaining previous contents.
327  * \param new_x_min The "x" coordinates of new left most side of grid.
328  * \param new_x_max The "x" coordinates of new right most side of grid.
329  * \param new_y_min The "y" coordinates of new top most side of grid.
330  * \param new_y_max The "y" coordinates of new bottom most side of grid.
331  * \param new_cells_default_value The value of the new cells, tipically 0.5.
332  * \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.
333  * \sa setSize
334  */
335  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;
336 
337  /** Returns the area of the gridmap, in square meters */
338  inline double getArea() const { return size_x*size_y*square(resolution); }
339 
340  /** Returns the horizontal size of grid map in cells count.
341  */
342  inline unsigned int getSizeX() const { return size_x; }
343 
344  /** Returns the vertical size of grid map in cells count.
345  */
346  inline unsigned int getSizeY() const { return size_y; }
347 
348  /** Returns the "x" coordinate of left side of grid map.
349  */
350  inline float getXMin() const { return x_min; }
351 
352  /** Returns the "x" coordinate of right side of grid map.
353  */
354  inline float getXMax() const { return x_max; }
355 
356  /** Returns the "y" coordinate of top side of grid map.
357  */
358  inline float getYMin() const { return y_min; }
359 
360  /** Returns the "y" coordinate of bottom side of grid map.
361  */
362  inline float getYMax() const { return y_max; }
363 
364  /** Returns the resolution of the grid map.
365  */
366  inline float getResolution() const { return resolution; }
367 
368  /** Transform a coordinate value into a cell index.
369  */
370  inline int x2idx(float x) const { return static_cast<int>((x-x_min)/resolution ); }
371  inline int y2idx(float y) const { return static_cast<int>((y-y_min)/resolution ); }
372 
373  inline int x2idx(double x) const { return static_cast<int>((x-x_min)/resolution ); }
374  inline int y2idx(double y) const { return static_cast<int>((y-y_min)/resolution ); }
375 
376  /** Transform a cell index into a coordinate value.
377  */
378  inline float idx2x(const size_t cx) const { return x_min+(cx+0.5f)*resolution; }
379  inline float idx2y(const size_t cy) const { return y_min+(cy+0.5f)*resolution; }
380 
381  /** Transform a coordinate value into a cell index, using a diferent "x_min" value
382  */
383  inline int x2idx(float x,float x_min) const { return static_cast<int>((x-x_min)/resolution ); }
384  inline int y2idx(float y, float y_min) const { return static_cast<int>((y-y_min)/resolution ); }
385 
386  /** Scales an integer representation of the log-odd into a real valued probability in [0,1], using p=exp(l)/(1+exp(l))
387  */
388  static inline float l2p(const cellType l)
389  {
390  return m_logodd_lut.l2p(l);
391  }
392 
393  /** Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l))
394  */
395  static inline uint8_t l2p_255(const cellType l)
396  {
397  return m_logodd_lut.l2p_255(l);
398  }
399 
400  /** Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the valid range of cellType.
401  */
402  static inline cellType p2l(const float p)
403  {
404  return m_logodd_lut.p2l(p);
405  }
406 
407  /** Change the contents [0,1] of a cell, given its index.
408  */
409  inline void setCell(int x,int y,float value)
410  {
411  // The x> comparison implicitly holds if x<0
412  if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
413  return;
414  else map[x+y*size_x]=p2l(value);
415  }
416 
417  /** Read the real valued [0,1] contents of a cell, given its index.
418  */
419  inline float getCell(int x,int y) const
420  {
421  // The x> comparison implicitly holds if x<0
422  if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
423  return 0.5f;
424  else return l2p(map[x+y*size_x]);
425  }
426 
427  /** Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
428  */
429  inline cellType *getRow( int cy ) { if (cy<0 || static_cast<unsigned int>(cy)>=size_y) return NULL; else return &map[0+cy*size_x]; }
430 
431  /** Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
432  */
433  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]; }
434 
435  /** Change the contents [0,1] of a cell, given its coordinates.
436  */
437  inline void setPos(float x,float y,float value) { setCell(x2idx(x),y2idx(y),value); }
438 
439  /** Read the real valued [0,1] contents of a cell, given its coordinates.
440  */
441  inline float getPos(float x,float y) const { return getCell(x2idx(x),y2idx(y)); }
442 
443  /** Returns "true" if cell is "static", i.e.if its occupancy is below a given threshold.
444  */
445  inline bool isStaticPos(float x,float y,float threshold = 0.7f) const { return isStaticCell(x2idx(x),y2idx(y),threshold); }
446  inline bool isStaticCell(int cx,int cy,float threshold = 0.7f) const { return (getCell(cx,cy)<=threshold); }
447 
448  /** Change a cell in the "basis" maps.Used for Voronoi calculation.
449  */
450  inline void setBasisCell(int x,int y,uint8_t value)
451  {
452  uint8_t *cell=m_basis_map.cellByIndex(x,y);
453 #ifdef _DEBUG
454  ASSERT_ABOVEEQ_(x,0)
455  ASSERT_ABOVEEQ_(y,0)
456  ASSERT_BELOWEQ_(x,int(m_basis_map.getSizeX()))
457  ASSERT_BELOWEQ_(y,int(m_basis_map.getSizeY()))
458 #endif
459  *cell = value;
460  }
461 
462  /** Reads a cell in the "basis" maps.Used for Voronoi calculation.
463  */
464  inline unsigned char getBasisCell(int x,int y) const
465  {
466  const uint8_t *cell=m_basis_map.cellByIndex(x,y);
467 #ifdef _DEBUG
468  ASSERT_ABOVEEQ_(x,0)
469  ASSERT_ABOVEEQ_(y,0)
470  ASSERT_BELOWEQ_(x,int(m_basis_map.getSizeX()))
471  ASSERT_BELOWEQ_(y,int(m_basis_map.getSizeY()))
472 #endif
473  return *cell;
474  }
475 
476  /** Used for returning entropy related information
477  * \sa computeEntropy
478  */
479  struct MAPS_IMPEXP TEntropyInfo
480  {
481  TEntropyInfo() : H(0),I(0),mean_H(0),mean_I(0),effectiveMappedArea(0),effectiveMappedCells(0)
482  {
483  }
484 
485  /** 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>
486  */
487  double H;
488 
489  /** The target variable for absolute "information", defining I(x) = 1 - H(x)
490  */
491  double I;
492 
493  /** The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells)
494  */
495  double mean_H;
496 
497  /** The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (cells)
498  */
499  double mean_I;
500 
501  /** The target variable for the area of cells with information, i.e. p(x)!=0.5
502  */
503  double effectiveMappedArea;
504 
505  /** The mapped area in cells.
506  */
507  unsigned long effectiveMappedCells;
508  };
509 
510  /** With this struct options are provided to the observation insertion process.
511  * \sa CObservation::insertIntoGridMap
512  */
513  class MAPS_IMPEXP TInsertionOptions : public mrpt::utils::CLoadableOptions
514  {
515  public:
516  /** Initilization of default parameters
517  */
518  TInsertionOptions( );
519 
520  /** This method load the options from a ".ini" file.
521  * Only those parameters found in the given "section" and having
522  * the same name that the variable are loaded. Those not found in
523  * the file will stay with their previous values (usually the default
524  * values loaded at initialization). An example of an ".ini" file:
525  * \code
526  * [section]
527  * resolution=0.10 ; blah blah...
528  * modeSelection=1 ; 0=blah, 1=blah,...
529  * \endcode
530  */
531  void loadFromConfigFile(
532  const mrpt::utils::CConfigFileBase &source,
533  const std::string &section);
534 
535  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
536  */
537  void dumpToTextStream(CStream &out) const;
538 
539 
540  /** The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map!
541  */
542  float mapAltitude;
543 
544  /** The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true.
545  */
546  bool useMapAltitude;
547 
548  /** The largest distance at which cells will be updated (Default 15 meters)
549  */
550  float maxDistanceInsertion;
551 
552  /** A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0.8)
553  */
554  float maxOccupancyUpdateCertainty;
555 
556  /** 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.
557  */
558  bool considerInvalidRangesAsFreeSpace;
559 
560  /** Specify the decimation of the range scan (default=1 : take all the range values!)
561  */
562  uint16_t decimation;
563 
564  /** The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, then processed by calls to this class (default=0). */
565  float horizontalTolerance;
566 
567  /** Gaussian sigma of the filter used in getAsImageFiltered (for features detection) (Default=1) (0:Disabled) */
568  float CFD_features_gaussian_size;
569 
570  /** Size of the Median filter used in getAsImageFiltered (for features detection) (Default=3) (0:Disabled) */
571  float CFD_features_median_size;
572 
573  bool wideningBeamsWithDistance; //!< Enabled: Rays widen with distance to approximate the real behavior of lasers, disabled: insert rays as simple lines (Default=false)
574 
575  };
576 
577  /** With this struct options are provided to the observation insertion process.
578  * \sa CObservation::insertIntoGridMap
579  */
580  TInsertionOptions insertionOptions;
581 
582  /** The type for selecting a likelihood computation method
583  */
584  enum TLikelihoodMethod
585  {
586  lmMeanInformation = 0,
587  lmRayTracing,
588  lmConsensus,
589  lmCellsDifference,
590  lmLikelihoodField_Thrun,
591  lmLikelihoodField_II,
592  lmConsensusOWA
593  };
594 
595  /** With this struct options are provided to the observation likelihood computation process.
596  */
597  class MAPS_IMPEXP TLikelihoodOptions : public mrpt::utils::CLoadableOptions
598  {
599  public:
600  /** Initilization of default parameters
601  */
602  TLikelihoodOptions();
603 
604  /** This method load the options from a ".ini" file.
605  * Only those parameters found in the given "section" and having
606  * the same name that the variable are loaded. Those not found in
607  * the file will stay with their previous values (usually the default
608  * values loaded at initialization). An example of an ".ini" file:
609  * \code
610  * [section]
611  * resolution=0.10 ; blah blah...
612  * modeSelection=1 ; 0=blah, 1=blah,...
613  * \endcode
614  */
615  void loadFromConfigFile(
616  const mrpt::utils::CConfigFileBase &source,
617  const std::string &section);
618 
619  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
620  */
621  void dumpToTextStream(CStream &out) const;
622 
623  /** The selected method to compute an observation likelihood.
624  */
625  TLikelihoodMethod likelihoodMethod;
626 
627  /** [LikelihoodField] The laser range "sigma" used in computations; Default value = 0.35
628  */
629  float LF_stdHit;
630 
631  /** [LikelihoodField] Ratios of the hit/random components of the likelihood; Default values=0.95/0.5
632  */
633  float LF_zHit, LF_zRandom;
634 
635  /** [LikelihoodField] The max. range of the sensor (def=81meters)
636  */
637  float LF_maxRange;
638 
639  /** [LikelihoodField] The decimation of the points in a scan, default=1 == no decimation.
640  */
641  uint32_t LF_decimation;
642 
643  /** [LikelihoodField] The max. distance for searching correspondences around each sensed point
644  */
645  float LF_maxCorrsDistance;
646 
647  /** [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".
648  */
649  bool LF_alternateAverageMethod;
650 
651  /** [MI] The exponent in the MI likelihood computation. Default value = 5
652  */
653  float MI_exponent;
654 
655  /** [MI] The scan rays decimation: at every N rays, one will be used to compute the MI:
656  */
657  uint32_t MI_skip_rays;
658 
659  /** [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.
660  */
661  float MI_ratio_max_distance;
662 
663  /** [rayTracing] If true (default), the rayTracing method will ignore measured ranges shorter than the simulated ones.
664  */
665  bool rayTracing_useDistanceFilter;
666 
667  /** [rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to use all the sensed ranges.
668  */
669  int32_t rayTracing_decimation;
670 
671  /** [rayTracing] The laser range sigma.
672  */
673  float rayTracing_stdHit;
674 
675  /** [Consensus] The down-sample ratio of ranges (default=1, consider all the ranges)
676  */
677  int32_t consensus_takeEachRange;
678 
679  /** [Consensus] The power factor for the likelihood (default=5)
680  */
681  float consensus_pow;
682 
683  /** [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.
684  */
685  std::vector<float> OWA_weights;
686 
687  /** Enables the usage of a cache of likelihood values (for LF methods), if set to true (default=false).
688  */
689  bool enableLikelihoodCache;
690 
691  } likelihoodOptions;
692 
693  /** Auxiliary private class.
694  */
695  typedef std::pair<double,CPoint2D> TPairLikelihoodIndex;
696 
697  /** Some members of this struct will contain intermediate or output data after calling "computeObservationLikelihood" for some likelihood functions.
698  */
699  class TLikelihoodOutput
700  {
701  public:
702  TLikelihoodOutput() : OWA_pairList(), OWA_individualLikValues()
703  {}
704 
705  /** [OWA method] This will contain the ascending-ordered list of pairs:(likelihood values, 2D point in map coordinates).
706  */
707  std::vector<TPairLikelihoodIndex> OWA_pairList;
708 
709  /** [OWA method] This will contain the ascending-ordered list of likelihood values for individual range measurements in the scan.
710  */
711  std::vector<double> OWA_individualLikValues;
712 
713  } likelihoodOutputs;
714 
715  /** Performs a downsampling of the gridmap, by a given factor: resolution/=ratio
716  */
717  void subSample( int downRatio );
718 
719  /** Computes the entropy and related values of this grid map.
720  * The entropy is computed as the summed entropy of each cell, taking them as discrete random variables following a Bernoulli distribution:
721  * \param info The output information is returned here.
722  */
723  void computeEntropy( TEntropyInfo &info ) const;
724 
725  /** @name Voronoi methods
726  @{ */
727 
728  /** Build the Voronoi diagram of the grid map.
729  * \param threshold The threshold for binarizing the map.
730  * \param robot_size Size in "units" (meters) of robot, approx.
731  * \param x1 Left coordinate of area to be computed. Default, entire map.
732  * \param x2 Right coordinate of area to be computed. Default, entire map.
733  * \param y1 Top coordinate of area to be computed. Default, entire map.
734  * \param y2 Bottom coordinate of area to be computed. Default, entire map.
735  * \sa findCriticalPoints
736  */
737  void buildVoronoiDiagram(float threshold, float robot_size,int x1=0,int x2=0, int y1=0,int y2=0);
738 
739  /** Reads a the clearance of a cell (in centimeters), after building the Voronoi diagram with \a buildVoronoiDiagram */
740  inline uint16_t getVoroniClearance(int cx,int cy) const
741  {
742 #ifdef _DEBUG
743  ASSERT_ABOVEEQ_(cx,0)
744  ASSERT_ABOVEEQ_(cy,0)
745  ASSERT_BELOWEQ_(cx,int(m_voronoi_diagram.getSizeX()))
746  ASSERT_BELOWEQ_(cy,int(m_voronoi_diagram.getSizeY()))
747 #endif
748  const uint16_t *cell=m_voronoi_diagram.cellByIndex(cx,cy);
749  return *cell;
750  }
751 
752  protected:
753  /** Used to set the clearance of a cell, while building the Voronoi diagram. */
754  inline void setVoroniClearance(int cx,int cy,uint16_t dist)
755  {
756  uint16_t *cell=m_voronoi_diagram.cellByIndex(cx,cy);
757 #ifdef _DEBUG
758  ASSERT_ABOVEEQ_(cx,0)
759  ASSERT_ABOVEEQ_(cy,0)
760  ASSERT_BELOWEQ_(cx,int(m_voronoi_diagram.getSizeX()))
761  ASSERT_BELOWEQ_(cy,int(m_voronoi_diagram.getSizeY()))
762 #endif
763  *cell = dist;
764  }
765 
766  public:
767 
768  /** Return the auxiliary "basis" map built while building the Voronoi diagram \sa buildVoronoiDiagram */
769  inline const CDynamicGrid<uint8_t> & getBasisMap() const { return m_basis_map; }
770 
771  /** Return the Voronoi diagram; each cell contains the distance to its closer obstacle, or 0 if not part of the Voronoi diagram \sa buildVoronoiDiagram */
772  inline const CDynamicGrid<uint16_t> & getVoronoiDiagram() const { return m_voronoi_diagram; }
773 
774  /** Builds a list with the critical points from Voronoi diagram, which must
775  * must be built before calling this method.
776  * \param filter_distance The minimum distance between two critical points.
777  * \sa buildVoronoiDiagram
778  */
779  void findCriticalPoints( float filter_distance );
780 
781  /** @} */ // End of Voronoi methods
782 
783 
784  /** Compute the clearance of a given cell, and returns its two first
785  * basis (closest obstacle) points.Used to build Voronoi and critical points.
786  * \return The clearance of the cell, in 1/100 of "cell".
787  * \param cx The cell index
788  * \param cy The cell index
789  * \param basis_x Target buffer for coordinates of basis, having a size of two "ints".
790  * \param basis_y Target buffer for coordinates of basis, having a size of two "ints".
791  * \param nBasis The number of found basis: Can be 0,1 or 2.
792  * \param GetContourPoint If "true" the basis are not returned, but the closest free cells.Default at false.
793  * \sa Build_VoronoiDiagram
794  */
795  int computeClearance( int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint = false ) const;
796 
797  /** An alternative method for computing the clearance of a given location (in meters).
798  * \return The clearance (distance to closest OCCUPIED cell), in meters.
799  */
800  float computeClearance( float x, float y, float maxSearchDistance ) const;
801 
802  /** Compute the 'cost' of traversing a segment of the map according to the occupancy of traversed cells.
803  * \return This returns '1-mean(traversed cells occupancy)', i.e. 0.5 for unknown cells, 1 for a free path.
804  */
805  float computePathCost( float x1, float y1, float x2, float y2 ) const;
806 
807 
808  /** \name Sensor simulators
809  @{
810  */
811 
812  /** Simulates a laser range scan into the current grid map.
813  * The simulated scan is stored in a CObservation2DRangeScan object, which is also used
814  * to pass some parameters: all previously stored characteristics (as aperture,...) are
815  * taken into account for simulation. Only a few more parameters are needed. Additive gaussian noise can be optionally added to the simulated scan.
816  * \param inout_Scan [IN/OUT] This must be filled with desired parameters before calling, and will contain the scan samples on return.
817  * \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.
818  * \param threshold [IN] The minimum occupancy threshold to consider a cell to be occupied, for example 0.5.
819  * \param N [IN] The count of range scan "rays", by default to 361.
820  * \param noiseStd [IN] The standard deviation of measurement noise. If not desired, set to 0.
821  * \param decimation [IN] The rays that will be simulated are at indexes: 0, D, 2D, 3D, ... Default is D=1
822  * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians).
823  *
824  * \sa sonarSimulator
825  */
826  void laserScanSimulator(
827  CObservation2DRangeScan &inout_Scan,
828  const CPose2D &robotPose,
829  float threshold = 0.5f,
830  size_t N = 361,
831  float noiseStd = 0,
832  unsigned int decimation = 1,
833  float angleNoiseStd = DEG2RAD(0) ) const;
834 
835  /** Simulates the observations of a sonar rig into the current grid map.
836  * The simulated ranges are stored in a CObservationRange object, which is also used
837  * to pass in some needed parameters, as the poses of the sonar sensors onto the mobile robot.
838  * \param inout_observation [IN/OUT] This must be filled with desired parameters before calling, and will contain the simulated ranges on return.
839  * \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.
840  * \param threshold [IN] The minimum occupancy threshold to consider a cell to be occupied, for example 0.5.
841  * \param rangeNoiseStd [IN] The standard deviation of measurement noise. If not desired, set to 0.
842  * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians).
843  *
844  * \sa laserScanSimulator
845  */
846  void sonarSimulator(
847  CObservationRange &inout_observation,
848  const CPose2D &robotPose,
849  float threshold = 0.5f,
850  float rangeNoiseStd = 0,
851  float angleNoiseStd = DEG2RAD(0) ) const;
852 
853  /** Simulate just one "ray" in the grid map. This method is used internally to sonarSimulator and laserScanSimulator.
854  */
855  inline void simulateScanRay(
856  const double x,const double y,const double angle_direction,
857  float &out_range,bool &out_valid,
858  const unsigned int max_ray_len,
859  const float threshold_free=0.5f,
860  const double noiseStd=0, const double angleNoiseStd=0 ) const;
861 
862 
863  /** @} */
864 
865  /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map.
866  * See "likelihoodOptions" for configuration parameters.
867  *
868  * \param takenFrom The robot's pose the observation is supposed to be taken from.
869  * \param obs The observation.
870  * \return This method returns a likelihood in the range [0,1].
871  *
872  * Used in particle filter algorithms, see: CMultiMetricMapPDF::prediction_and_update
873  *
874  * \sa likelihoodOptions, likelihoodOutputs
875  */
876  double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
877 
878  /** 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).
879  * \param obs The observation.
880  * \sa computeObservationLikelihood
881  */
882  bool canComputeObservationLikelihood( const CObservation *obs );
883 
884  /** Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
885  * \param pm The points map
886  * \param relativePose The relative pose of the points map in this map's coordinates, or NULL for (0,0,0).
887  * See "likelihoodOptions" for configuration parameters.
888  */
889  double computeLikelihoodField_Thrun( const CPointsMap *pm, const CPose2D *relativePose = NULL);
890 
891  /** Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
892  * \param pm The points map
893  * \param relativePose The relative pose of the points map in this map's coordinates, or NULL for (0,0,0).
894  * See "likelihoodOptions" for configuration parameters.
895  */
896  double computeLikelihoodField_II( const CPointsMap *pm, const CPose2D *relativePose = NULL);
897 
898  /** Saves the gridmap as a graphical file (BMP,PNG,...).
899  * The format will be derived from the file extension (see CImage::saveToFile )
900  * \return False on any error.
901  */
902  bool saveAsBitmapFile(const std::string &file) const;
903 
904  /** Saves a composite image with two gridmaps and lines representing a set of correspondences between them.
905  * \sa saveAsEMFTwoMapsWithCorrespondences
906  * \return False on any error.
907  */
908  static bool saveAsBitmapTwoMapsWithCorrespondences(
909  const std::string &fileName,
910  const COccupancyGridMap2D *m1,
911  const COccupancyGridMap2D *m2,
912  const TMatchingPairList &corrs);
913 
914  /** Saves a composite image with two gridmaps and numbers for the correspondences between them.
915  * \sa saveAsBitmapTwoMapsWithCorrespondences
916  * \return False on any error.
917  */
918  static bool saveAsEMFTwoMapsWithCorrespondences(
919  const std::string &fileName,
920  const COccupancyGridMap2D *m1,
921  const COccupancyGridMap2D *m2,
922  const TMatchingPairList &corrs);
923 
924  /** Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel is 1 cell, and with an overlay of landmarks.
925  * \note The template parameter CLANDMARKSMAP is assumed to be mrpt::slam::CLandmarksMap normally.
926  * \return False on any error.
927  */
928  template <class CLANDMARKSMAP>
929  bool saveAsBitmapFileWithLandmarks(
930  const std::string &file,
931  const CLANDMARKSMAP *landmarks,
932  bool addTextLabels = false,
933  const mrpt::utils::TColor &marks_color = mrpt::utils::TColor(0,0,255) ) const
934  {
935  MRPT_START
936  CImage img(1,1,3);
937  getAsImageFiltered( img, false, true ); // in RGB
938  const bool topleft = img.isOriginTopLeft();
939  for (unsigned int i=0;i<landmarks->landmarks.size();i++)
940  {
941  const typename CLANDMARKSMAP::landmark_type *lm = landmarks->landmarks.get( i );
942  int px = x2idx( lm->pose_mean.x );
943  int py = topleft ? size_y-1- y2idx( lm->pose_mean.y ) : y2idx( lm->pose_mean.y );
944  img.rectangle( px - 7, (py + 7), px +7, (py -7), marks_color );
945  img.rectangle( px - 6, (py + 6), px +6, (py -6), marks_color );
946  if (addTextLabels)
947  img.textOut(px,py-8,format("%u",i), TColor::black);
948  }
949  return img.saveToFile(file.c_str() );
950  MRPT_END
951  }
952 
953 
954  /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true)
955  * 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.
956  * \sa getAsImageFiltered
957  */
958  void getAsImage( utils::CImage &img, bool verticalFlip = false, bool forceRGB=false, bool tricolor = false) const;
959 
960  /** 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
961  * 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.
962  * \sa getAsImage
963  */
964  void getAsImageFiltered( utils::CImage &img, bool verticalFlip = false, bool forceRGB=false) const;
965 
966  /** 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)
967  */
968  void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
969 
970  /** Returns true upon map construction or after calling clear(), the return
971  * changes to false upon successful insertObservation() or any other method to load data in the map.
972  */
973  bool isEmpty() const;
974 
975  /** Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
976  * \param file The file to be loaded.
977  * \param resolution The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed.
978  * \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.
979  * \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.
980  * \return False on any error.
981  * \sa loadFromBitmap
982  */
983  bool loadFromBitmapFile(const std::string &file, float resolution, float xCentralPixel = -1, float yCentralPixel =-1 );
984 
985  /** Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
986  * \param img The image. Only a grayscale image will be used, so RGB components will be mixed if a color image is passed.
987  * \param resolution The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed.
988  * \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.
989  * \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.
990  * \return False on any error.
991  * \sa loadFromBitmapFile
992  */
993  bool loadFromBitmap(const mrpt::utils::CImage &img, float resolution, float xCentralPixel = -1, float yCentralPixel =-1 );
994 
995  /** See the base class for more details: In this class it is implemented as correspondences of the passed points map to occupied cells.
996  * NOTICE: That the "z" dimension is ignored in the points. Clip the points as appropiated if needed before calling this method.
997  *
998  * \sa computeMatching3DWith
999  */
1000  void computeMatchingWith2D(
1001  const CMetricMap *otherMap,
1002  const CPose2D &otherMapPose,
1003  float maxDistForCorrespondence,
1004  float maxAngularDistForCorrespondence,
1005  const CPose2D &angularDistPivotPoint,
1006  TMatchingPairList &correspondences,
1007  float &correspondencesRatio,
1008  float *sumSqrDist = NULL,
1009  bool onlyKeepTheClosest = false,
1010  bool onlyUniqueRobust = false,
1011  const size_t decimation_other_map_points = 1,
1012  const size_t offset_other_map_points = 0 ) const;
1013 
1014 
1015  /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
1016  * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
1017  * \param otherMap [IN] The other map to compute the matching with.
1018  * \param otherMapPose [IN] The 6D pose of the other map as seen from "this".
1019  * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
1020  * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
1021  *
1022  * \return The matching ratio [0,1]
1023  * \sa computeMatchingWith2D
1024  */
1025  float compute3DMatchingRatio(
1026  const CMetricMap *otherMap,
1027  const CPose3D &otherMapPose,
1028  float minDistForCorr = 0.10f,
1029  float minMahaDistForCorr = 2.0f
1030  ) const;
1031 
1032  /** 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).
1033  */
1034  void saveMetricMapRepresentationToFile(
1035  const std::string &filNamePrefix
1036  ) const;
1037 
1038  /** The structure used to store the set of Voronoi diagram
1039  * critical points.
1040  * \sa findCriticalPoints
1041  */
1042  struct MAPS_IMPEXP TCriticalPointsList
1043  {
1044  TCriticalPointsList() : x(),y(),clearance(),x_basis1(),y_basis1(),x_basis2(),y_basis2()
1045  {}
1046 
1047  /** The coordinates of critical point.
1048  */
1049  std::vector<int> x,y;
1050  /** The clearance of critical points, in 1/100 of cells.
1051  */
1052  std::vector<int> clearance;
1053  /** Their two first basis points coordinates.
1054  */
1055  std::vector<int> x_basis1,y_basis1, x_basis2,y_basis2;
1056  } CriticalPointsList;
1057 
1058 
1059  private:
1060  /** Returns a byte with the occupancy of the 8 sorrounding cells.
1061  * \param cx The cell index
1062  * \param cy The cell index
1063  * \sa direction2idx
1064  */
1065  inline unsigned char GetNeighborhood( int cx, int cy ) const;
1066 
1067  /** Used to store the 8 possible movements from a cell to the
1068  * sorrounding ones.Filled in the constructor.
1069  * \sa direction2idx
1070  */
1071  int direccion_vecino_x[8],direccion_vecino_y[8];
1072 
1073  /** Returns the index [0,7] of the given movement, or
1074  * -1 if invalid one.
1075  * \sa direccion_vecino_x,direccion_vecino_y,GetNeighborhood
1076  */
1077  int direction2idx(int dx, int dy);
1078  };
1079 
1080 
1081  bool operator <(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2);
1082 
1083  } // End of namespace
1084 } // End of namespace
1085 
1086 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013