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



Page generated by Doxygen 1.8.11 for MRPT 1.3.2 SVN: at Wed May 25 02:34:21 UTC 2016