Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 #pragma once
10 
13 #include <mrpt/img/CImage.h>
15 #include <mrpt/maps/CMetricMap.h>
19 #include <mrpt/poses/poses_frwds.h>
22 #include <mrpt/obs/obs_frwds.h>
24 
25 #include <mrpt/config.h>
26 #if ( \
27  !defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && \
28  !defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS)) || \
29  (defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && \
30  defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS))
31 #error One of OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS or OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS must be defined.
32 #endif
33 
34 namespace mrpt
35 {
36 namespace maps
37 {
38 /** A class for storing an occupancy grid map.
39  * COccupancyGridMap2D is a class for storing a metric map
40  * representation in the form of a probabilistic occupancy
41  * grid map: value of 0 means certainly occupied, 1 means
42  * a certainly empty cell. Initially 0.5 means uncertainty.
43  *
44  * The cells keep the log-odd representation of probabilities instead of the
45  *probabilities themselves.
46  * More details can be found at http://www.mrpt.org/Occupancy_Grids
47  *
48  * The algorithm for updating the grid from a laser scanner can optionally take
49  *into account the progressive widening of the beams, as
50  * described in [this page](http://www.mrpt.org/Occupancy_Grids)
51  *
52  * Some implemented methods are:
53  * - Update of individual cells
54  * - Insertion of observations
55  * - Voronoi diagram and critical points (\a buildVoronoiDiagram)
56  * - Saving and loading from/to a bitmap
57  * - Laser scans simulation for the map contents
58  * - Entropy and information methods (See computeEntropy)
59  *
60  * \ingroup mrpt_maps_grp
61  **/
63 // Inherit from the corresponding specialization of CLogOddsGridMap2D<>:
64 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
65  public CLogOddsGridMap2D<int8_t>
66 #else
67  public CLogOddsGridMap2D<int16_t>
68 #endif
69 {
71  public:
72 /** The type of the map cells: */
73 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
74  using cellType = int8_t;
75  using cellTypeUnsigned = uint8_t;
76 #else
77  using cellType = int16_t;
79 #endif
80 
81  /** Discrete to float conversion factors: The min/max values of the integer
82  * cell type, eg.[0,255] or [0,65535] */
89 
90  /** (Default:1.0) Can be set to <1 if a more fine raytracing is needed in
91  * sonarSimulator() and laserScanSimulator(), or >1 to speed it up. */
93 
94  protected:
95  friend class CMultiMetricMap;
96  friend class CMultiMetricMapPDF;
97 
98  /** Frees the dynamic memory buffers of map. */
99  void freeMap();
100  /** Lookup tables for log-odds */
102 
103  /** Store of cell occupancy values. Order: row by row, from left to right */
104  std::vector<cellType> map;
105  /** The size of the grid in cells */
107  /** The limits of the grid in "units" (meters) */
108  float x_min, x_max, y_min, y_max;
109  /** Cell size, i.e. resolution of the grid map. */
110  float resolution;
111 
112  /** Auxiliary variables to speed up the computation of observation
113  * likelihood values for LF method among others, at a high cost in memory
114  * (see TLikelihoodOptions::enableLikelihoodCache). */
115  std::vector<double> precomputedLikelihood;
117 
118  /** Used for Voronoi calculation.Same struct as "map", but contains a "0" if
119  * not a basis point. */
121 
122  /** Used to store the Voronoi diagram.
123  * Contains the distance of each cell to its closer obstacles
124  * in 1/100th distance units (i.e. in centimeters), or 0 if not into the
125  * Voronoi diagram */
127 
128  /** True upon construction; used by isEmpty() */
130 
131  /** See base class */
132  virtual void OnPostSuccesfulInsertObs(
133  const mrpt::obs::CObservation*) override;
134 
135  /** The free-cells threshold used to compute the Voronoi diagram. */
137 
138  /** Entropy computation internal function: */
139  static double H(double p);
140  /** Internally used to speed-up entropy calculation */
141  static std::vector<float> entropyTable;
142 
143  /** Change the contents [0,1] of a cell, given its index */
144  inline void setCell_nocheck(int x, int y, float value)
145  {
146  map[x + y * size_x] = p2l(value);
147  }
148 
149  /** Read the real valued [0,1] contents of a cell, given its index */
150  inline float getCell_nocheck(int x, int y) const
151  {
152  return l2p(map[x + y * size_x]);
153  }
154  /** Changes a cell by its absolute index (Do not use it normally) */
155  inline void setRawCell(unsigned int cellIndex, cellType b)
156  {
157  if (cellIndex < size_x * size_y) map[cellIndex] = b;
158  }
159 
160  /** One of the methods that can be selected for implementing
161  * "computeObservationLikelihood" (This method is the Range-Scan Likelihood
162  * Consensus for gridmaps, see the ICRA2007 paper by Blanco et al.) */
164  const mrpt::obs::CObservation* obs,
165  const mrpt::poses::CPose2D& takenFrom);
166  /** One of the methods that can be selected for implementing
167  * "computeObservationLikelihood". TODO: This method is described in.... */
169  const mrpt::obs::CObservation* obs,
170  const mrpt::poses::CPose2D& takenFrom);
171  /** One of the methods that can be selected for implementing
172  * "computeObservationLikelihood" */
174  const mrpt::obs::CObservation* obs,
175  const mrpt::poses::CPose2D& takenFrom);
176  /** One of the methods that can be selected for implementing
177  * "computeObservationLikelihood" */
179  const mrpt::obs::CObservation* obs,
180  const mrpt::poses::CPose2D& takenFrom);
181  /** One of the methods that can be selected for implementing
182  * "computeObservationLikelihood" */
184  const mrpt::obs::CObservation* obs,
185  const mrpt::poses::CPose2D& takenFrom);
186  /** One of the methods that can be selected for implementing
187  * "computeObservationLikelihood".*/
189  const mrpt::obs::CObservation* obs,
190  const mrpt::poses::CPose2D& takenFrom);
191  /** One of the methods that can be selected for implementing
192  * "computeObservationLikelihood". */
194  const mrpt::obs::CObservation* obs,
195  const mrpt::poses::CPose2D& takenFrom);
196 
197  /** Clear the map: It set all cells to their default occupancy value (0.5),
198  * without changing the resolution (the grid extension is reset to the
199  * default values). */
200  virtual void internal_clear() override;
201 
202  /** Insert the observation information into this map.
203  *
204  * \param obs The observation
205  * \param robotPose The 3D pose of the robot mobile base in the map
206  * reference system, or nullptr (default) if you want to use
207  * CPose2D(0,0,deg)
208  *
209  * After successfull execution, "lastObservationInsertionInfo" is updated.
210  *
211  * \sa insertionOptions, CObservation::insertObservationInto
212  */
213  virtual bool internal_insertObservation(
214  const mrpt::obs::CObservation* obs,
215  const mrpt::poses::CPose3D* robotPose = nullptr) override;
216 
217  public:
218  /** Read-only access to the raw cell contents (cells are in log-odd units)
219  */
220  const std::vector<cellType>& getRawMap() const { return this->map; }
221  /** Performs the Bayesian fusion of a new observation of a cell \sa
222  * updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free */
223  void updateCell(int x, int y, float v);
224 
225  /** An internal structure for storing data related to counting the new
226  * information apported by some observation */
228  {
230  bool enabled_ = false, double I_change_ = 0, int cellsUpdated_ = 0)
231  : enabled(enabled_),
232  I_change(I_change_),
233  cellsUpdated(cellsUpdated_),
234  laserRaysSkip(1)
235  {
236  }
237  /** If set to false (default), this struct is not used. Set to true only
238  * when measuring the info of an observation. */
239  bool enabled;
240  /** The cummulative change in Information: This is updated only from the
241  * "updateCell" method. */
242  double I_change;
243  /** The cummulative updated cells count: This is updated only from the
244  * "updateCell" method. */
246  /** In this mode, some laser rays can be skips to speep-up */
249 
250  /** Fills all the cells with a default value. */
251  void fill(float default_value = 0.5f);
252 
253  /** Constructor */
255  float min_x = -20.0f, float max_x = 20.0f, float min_y = -20.0f,
256  float max_y = 20.0f, float resolution = 0.05f);
257  /** Destructor */
258  virtual ~COccupancyGridMap2D();
259 
260  /** Change the size of gridmap, erasing all its previous contents.
261  * \param x_min The "x" coordinates of left most side of grid.
262  * \param x_max The "x" coordinates of right most side of grid.
263  * \param y_min The "y" coordinates of top most side of grid.
264  * \param y_max The "y" coordinates of bottom most side of grid.
265  * \param resolution The new size of cells.
266  * \param default_value The value of cells, tipically 0.5.
267  * \sa ResizeGrid
268  */
269  void setSize(
270  float x_min, float x_max, float y_min, float y_max, float resolution,
271  float default_value = 0.5f);
272 
273  /** Change the size of gridmap, maintaining previous contents.
274  * \param new_x_min The "x" coordinates of new left most side of grid.
275  * \param new_x_max The "x" coordinates of new right most side of grid.
276  * \param new_y_min The "y" coordinates of new top most side of grid.
277  * \param new_y_max The "y" coordinates of new bottom most side of grid.
278  * \param new_cells_default_value The value of the new cells, tipically 0.5.
279  * \param additionalMargin If set to true (default), an additional margin of
280  * a few meters will be added to the grid, ONLY if the new coordinates are
281  * larger than current ones.
282  * \sa setSize
283  */
284  void resizeGrid(
285  float new_x_min, float new_x_max, float new_y_min, float new_y_max,
286  float new_cells_default_value = 0.5f,
287  bool additionalMargin = true) noexcept;
288 
289  /** Returns the area of the gridmap, in square meters */
290  inline double getArea() const
291  {
292  return size_x * size_y * mrpt::square(resolution);
293  }
294 
295  /** Returns the horizontal size of grid map in cells count */
296  inline unsigned int getSizeX() const { return size_x; }
297  /** Returns the vertical size of grid map in cells count */
298  inline unsigned int getSizeY() const { return size_y; }
299  /** Returns the "x" coordinate of left side of grid map */
300  inline float getXMin() const { return x_min; }
301  /** Returns the "x" coordinate of right side of grid map */
302  inline float getXMax() const { return x_max; }
303  /** Returns the "y" coordinate of top side of grid map */
304  inline float getYMin() const { return y_min; }
305  /** Returns the "y" coordinate of bottom side of grid map */
306  inline float getYMax() const { return y_max; }
307  /** Returns the resolution of the grid map */
308  inline float getResolution() const { return resolution; }
309  /** Transform a coordinate value into a cell index */
310  inline int x2idx(float x) const
311  {
312  return static_cast<int>((x - x_min) / resolution);
313  }
314  inline int y2idx(float y) const
315  {
316  return static_cast<int>((y - y_min) / resolution);
317  }
318 
319  inline int x2idx(double x) const
320  {
321  return static_cast<int>((x - x_min) / resolution);
322  }
323  inline int y2idx(double y) const
324  {
325  return static_cast<int>((y - y_min) / resolution);
326  }
327 
328  /** Transform a cell index into a coordinate value */
329  inline float idx2x(const size_t cx) const
330  {
331  return x_min + (cx + 0.5f) * resolution;
332  }
333  inline float idx2y(const size_t cy) const
334  {
335  return y_min + (cy + 0.5f) * resolution;
336  }
337 
338  /** Transform a coordinate value into a cell index, using a diferent "x_min"
339  * value */
340  inline int x2idx(float x, float xmin) const
341  {
342  return static_cast<int>((x - xmin) / resolution);
343  }
344  inline int y2idx(float y, float ymin) const
345  {
346  return static_cast<int>((y - ymin) / resolution);
347  }
348 
349  /** Scales an integer representation of the log-odd into a real valued
350  * probability in [0,1], using p=exp(l)/(1+exp(l)) */
351  static inline float l2p(const cellType l)
352  {
353  return get_logodd_lut().l2p(l);
354  }
355  /** Scales an integer representation of the log-odd into a linear scale
356  * [0,255], using p=exp(l)/(1+exp(l)) */
357  static inline uint8_t l2p_255(const cellType l)
358  {
359  return get_logodd_lut().l2p_255(l);
360  }
361  /** Scales a real valued probability in [0,1] to an integer representation
362  * of: log(p)-log(1-p) in the valid range of cellType */
363  static inline cellType p2l(const float p)
364  {
365  return get_logodd_lut().p2l(p);
366  }
367  /** Change the contents [0,1] of a cell, given its index */
368  inline void setCell(int x, int y, float value)
369  {
370  // The x> comparison implicitly holds if x<0
371  if (static_cast<unsigned int>(x) >= size_x ||
372  static_cast<unsigned int>(y) >= size_y)
373  return;
374  else
375  map[x + y * size_x] = p2l(value);
376  }
377 
378  /** Read the real valued [0,1] contents of a cell, given its index */
379  inline float getCell(int x, int y) const
380  {
381  // The x> comparison implicitly holds if x<0
382  if (static_cast<unsigned int>(x) >= size_x ||
383  static_cast<unsigned int>(y) >= size_y)
384  return 0.5f;
385  else
386  return l2p(map[x + y * size_x]);
387  }
388 
389  /** Access to a "row": mainly used for drawing grid as a bitmap efficiently,
390  * do not use it normally */
391  inline cellType* getRow(int cy)
392  {
393  if (cy < 0 || static_cast<unsigned int>(cy) >= size_y)
394  return nullptr;
395  else
396  return &map[0 + cy * size_x];
397  }
398 
399  /** Access to a "row": mainly used for drawing grid as a bitmap efficiently,
400  * do not use it normally */
401  inline const cellType* getRow(int cy) const
402  {
403  if (cy < 0 || static_cast<unsigned int>(cy) >= size_y)
404  return nullptr;
405  else
406  return &map[0 + cy * size_x];
407  }
408 
409  /** Change the contents [0,1] of a cell, given its coordinates */
410  inline void setPos(float x, float y, float value)
411  {
412  setCell(x2idx(x), y2idx(y), value);
413  }
414 
415  /** Read the real valued [0,1] contents of a cell, given its coordinates */
416  inline float getPos(float x, float y) const
417  {
418  return getCell(x2idx(x), y2idx(y));
419  }
420 
421  /** Returns "true" if cell is "static", i.e.if its occupancy is below a
422  * given threshold */
423  inline bool isStaticPos(float x, float y, float threshold = 0.7f) const
424  {
425  return isStaticCell(x2idx(x), y2idx(y), threshold);
426  }
427  inline bool isStaticCell(int cx, int cy, float threshold = 0.7f) const
428  {
429  return (getCell(cx, cy) <= threshold);
430  }
431 
432  /** Change a cell in the "basis" maps.Used for Voronoi calculation */
433  inline void setBasisCell(int x, int y, uint8_t value)
434  {
435  uint8_t* cell = m_basis_map.cellByIndex(x, y);
436 #ifdef _DEBUG
437  ASSERT_ABOVEEQ_(x, 0);
438  ASSERT_ABOVEEQ_(y, 0);
441 #endif
442  *cell = value;
443  }
444 
445  /** Reads a cell in the "basis" maps.Used for Voronoi calculation */
446  inline unsigned char getBasisCell(int x, int y) const
447  {
448  const uint8_t* cell = m_basis_map.cellByIndex(x, y);
449 #ifdef _DEBUG
450  ASSERT_ABOVEEQ_(x, 0);
451  ASSERT_ABOVEEQ_(y, 0);
454 #endif
455  return *cell;
456  }
457 
458  /** copy the gridmap contents, but not all the options, from another map
459  * instance */
460  void copyMapContentFrom(const COccupancyGridMap2D& otherMap);
461 
462  /** Used for returning entropy related information \sa computeEntropy */
464  {
465  /** The target variable for absolute entropy, computed
466  * as:<br><center>H(map)=Sum<sub>x,y</sub>{ -p(x,y)*ln(p(x,y))
467  * -(1-p(x,y))*ln(1-p(x,y)) }</center><br><br> */
468  double H{0};
469  /** The target variable for absolute "information", defining I(x) = 1 -
470  * H(x) */
471  double I{0};
472  /** The target variable for mean entropy, defined as entropy per cell:
473  * mean_H(map) = H(map) / (cells) */
474  double mean_H{0};
475  /** The target variable for mean information, defined as information per
476  * cell: mean_I(map) = I(map) / (cells) */
477  double mean_I{0};
478  /** The target variable for the area of cells with information, i.e.
479  * p(x)!=0.5 */
481  /** The mapped area in cells. */
482  unsigned long effectiveMappedCells{0};
483  };
484 
485  /** With this struct options are provided to the observation insertion
486  * process.
487  * \sa CObservation::insertIntoGridMap */
489  {
490  public:
491  /** Initilization of default parameters
492  */
494 
495  /** This method load the options from a ".ini" file.
496  * Only those parameters found in the given "section" and having
497  * the same name that the variable are loaded. Those not found in
498  * the file will stay with their previous values (usually the default
499  * values loaded at initialization). An example of an ".ini" file:
500  * \code
501  * [section]
502  * resolution=0.10 ; blah blah...
503  * modeSelection=1 ; 0=blah, 1=blah,...
504  * \endcode
505  */
506  void loadFromConfigFile(
508  const std::string& section) override; // See base docs
509  void dumpToTextStream(
510  std::ostream& out) const override; // See base docs
511 
512  /** The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for
513  * they to be inserted in this map! */
514  float mapAltitude;
515  /** The parameter "mapAltitude" has effect while inserting observations
516  * in the grid only if this is true. */
518  /** The largest distance at which cells will be updated (Default 15
519  * meters) */
521  /** A value in the range [0.5,1] used for updating cell with a bayesian
522  * approach (default 0.8) */
524  /** A value in the range [0.5,1] for updating a free cell. (default=0
525  * means use the same than maxOccupancyUpdateCertainty) */
527  /** Like maxFreenessUpdateCertainty, but for invalid ranges (no echo).
528  * (default=0 means same than maxOccupancyUpdateCertainty)*/
530  /** If set to true (default), invalid range values (no echo rays) as
531  * consider as free space until "maxOccupancyUpdateCertainty", but ONLY
532  * when the previous and next rays are also an invalid ray. */
534  /** Specify the decimation of the range scan (default=1 : take all the
535  * range values!) */
537  /** The tolerance in rads in pitch & roll for a laser scan to be
538  * considered horizontal, then processed by calls to this class
539  * (default=0). */
541  /** Gaussian sigma of the filter used in getAsImageFiltered (for
542  * features detection) (Default=1) (0:Disabled) */
544  /** Size of the Median filter used in getAsImageFiltered (for features
545  * detection) (Default=3) (0:Disabled) */
547  /** Enabled: Rays widen with distance to approximate the real behavior
548  * of lasers, disabled: insert rays as simple lines (Default=false) */
550  };
551 
552  /** With this struct options are provided to the observation insertion
553  * process \sa CObservation::insertIntoGridMap */
555 
556  /** The type for selecting a likelihood computation method */
558  {
566  // Remember: Update TEnumType below if new values are added here!
567  };
568 
569  /** With this struct options are provided to the observation likelihood
570  * computation process */
572  {
573  public:
574  /** Initilization of default parameters */
576 
577  /** This method load the options from a ".ini" file.
578  * Only those parameters found in the given "section" and having
579  * the same name that the variable are loaded. Those not found in
580  * the file will stay with their previous values (usually the default
581  * values loaded at initialization). An example of an ".ini" file:
582  * \code
583  * [section]
584  * resolution=0.10 ; blah blah...
585  * modeSelection=1 ; 0=blah, 1=blah,...
586  * \endcode
587  */
588  void loadFromConfigFile(
590  const std::string& section) override; // See base docs
591  void dumpToTextStream(
592  std::ostream& out) const override; // See base docs
593 
594  /** The selected method to compute an observation likelihood */
596  /** [LikelihoodField] The laser range "sigma" used in computations;
597  * Default value = 0.35 */
598  float LF_stdHit;
599  /** [LikelihoodField] */
601  /** [LikelihoodField] The max. range of the sensor (Default= 81 m) */
602  float LF_maxRange;
603  /** [LikelihoodField] The decimation of the points in a scan, default=1
604  * == no decimation */
606  /** [LikelihoodField] The max. distance for searching correspondences
607  * around each sensed point */
609  /** [LikelihoodField] (Default:false) Use `exp(dist^2/std^2)` instead of
610  * `exp(dist^2/std^2)` */
612  /** [LikelihoodField] Set this to "true" ot use an alternative method,
613  * where the likelihood of the whole range scan is computed by
614  * "averaging" of individual ranges, instead of by the "product". */
616  /** [MI] The exponent in the MI likelihood computation. Default value =
617  * 5 */
618  float MI_exponent;
619  /** [MI] The scan rays decimation: at every N rays, one will be used to
620  * compute the MI */
622  /** [MI] The ratio for the max. distance used in the MI computation and
623  * in the insertion of scans, e.g. if set to 2.0 the MI will use twice
624  * the distance that the update distance. */
626  /** [rayTracing] If true (default), the rayTracing method will ignore
627  * measured ranges shorter than the simulated ones. */
629  /** [rayTracing] One out of "rayTracing_decimation" rays will be
630  * simulated and compared only: set to 1 to use all the sensed ranges.
631  */
633  /** [rayTracing] The laser range sigma. */
635  /** [Consensus] The down-sample ratio of ranges (default=1, consider all
636  * the ranges) */
638  /** [Consensus] The power factor for the likelihood (default=5) */
640  /** [OWA] The sequence of weights to be multiplied to of the ordered
641  * list of likelihood values (first one is the largest); the size of
642  * this vector determines the number of highest likelihood values to
643  * fuse. */
644  std::vector<float> OWA_weights;
645 
646  /** Enables the usage of a cache of likelihood values (for LF methods),
647  * if set to true (default=false). */
650 
651  /** Auxiliary private class. */
652  using TPairLikelihoodIndex = std::pair<double, mrpt::math::TPoint2D>;
653 
654  /** Some members of this struct will contain intermediate or output data
655  * after calling "computeObservationLikelihood" for some likelihood
656  * functions */
658  {
659  public:
661  /** [OWA method] This will contain the ascending-ordered list of
662  * pairs:(likelihood values, 2D point in map coordinates). */
663  std::vector<TPairLikelihoodIndex> OWA_pairList;
664  /** [OWA method] This will contain the ascending-ordered list of
665  * likelihood values for individual range measurements in the scan. */
666  std::vector<double> OWA_individualLikValues;
668 
669  /** Performs a downsampling of the gridmap, by a given factor:
670  * resolution/=ratio */
671  void subSample(int downRatio);
672 
673  /** Computes the entropy and related values of this grid map.
674  * The entropy is computed as the summed entropy of each cell, taking them
675  * as discrete random variables following a Bernoulli distribution:
676  * \param info The output information is returned here */
677  void computeEntropy(TEntropyInfo& info) const;
678 
679  /** @name Voronoi methods
680  @{ */
681 
682  /** Build the Voronoi diagram of the grid map.
683  * \param threshold The threshold for binarizing the map.
684  * \param robot_size Size in "units" (meters) of robot, approx.
685  * \param x1 Left coordinate of area to be computed. Default, entire map.
686  * \param x2 Right coordinate of area to be computed. Default, entire map.
687  * \param y1 Top coordinate of area to be computed. Default, entire map.
688  * \param y2 Bottom coordinate of area to be computed. Default, entire map.
689  * \sa findCriticalPoints
690  */
691  void buildVoronoiDiagram(
692  float threshold, float robot_size, int x1 = 0, int x2 = 0, int y1 = 0,
693  int y2 = 0);
694 
695  /** Reads a the clearance of a cell (in centimeters), after building the
696  * Voronoi diagram with \a buildVoronoiDiagram */
697  inline uint16_t getVoroniClearance(int cx, int cy) const
698  {
699 #ifdef _DEBUG
700  ASSERT_ABOVEEQ_(cx, 0);
701  ASSERT_ABOVEEQ_(cy, 0);
704 #endif
705  const uint16_t* cell = m_voronoi_diagram.cellByIndex(cx, cy);
706  return *cell;
707  }
708 
709  protected:
710  /** Used to set the clearance of a cell, while building the Voronoi diagram.
711  */
712  inline void setVoroniClearance(int cx, int cy, uint16_t dist)
713  {
714  uint16_t* cell = m_voronoi_diagram.cellByIndex(cx, cy);
715 #ifdef _DEBUG
716  ASSERT_ABOVEEQ_(cx, 0);
717  ASSERT_ABOVEEQ_(cy, 0);
720 #endif
721  *cell = dist;
722  }
723 
724  public:
725  /** Return the auxiliary "basis" map built while building the Voronoi
726  * diagram \sa buildVoronoiDiagram */
728  {
729  return m_basis_map;
730  }
731 
732  /** Return the Voronoi diagram; each cell contains the distance to its
733  * closer obstacle, or 0 if not part of the Voronoi diagram \sa
734  * buildVoronoiDiagram */
736  const
737  {
738  return m_voronoi_diagram;
739  }
740 
741  /** Builds a list with the critical points from Voronoi diagram, which must
742  * must be built before calling this method.
743  * \param filter_distance The minimum distance between two critical points.
744  * \sa buildVoronoiDiagram
745  */
746  void findCriticalPoints(float filter_distance);
747 
748  /** @} */ // End of Voronoi methods
749 
750  /** Compute the clearance of a given cell, and returns its two first
751  * basis (closest obstacle) points.Used to build Voronoi and critical
752  * points.
753  * \return The clearance of the cell, in 1/100 of "cell".
754  * \param cx The cell index
755  * \param cy The cell index
756  * \param basis_x Target buffer for coordinates of basis, having a size of
757  * two "ints".
758  * \param basis_y Target buffer for coordinates of basis, having a size of
759  * 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
762  * closest free cells.Default at false.
763  * \sa Build_VoronoiDiagram
764  */
765  int computeClearance(
766  int cx, int cy, int* basis_x, int* basis_y, int* nBasis,
767  bool GetContourPoint = false) const;
768 
769  /** An alternative method for computing the clearance of a given location
770  * (in meters).
771  * \return The clearance (distance to closest OCCUPIED cell), in meters.
772  */
773  float computeClearance(float x, float y, float maxSearchDistance) const;
774 
775  /** Compute the 'cost' of traversing a segment of the map according to the
776  * occupancy of traversed cells.
777  * \return This returns '1-mean(traversed cells occupancy)', i.e. 0.5 for
778  * unknown cells, 1 for a free path.
779  */
780  float computePathCost(float x1, float y1, float x2, float y2) const;
781 
782  /** \name Sensor simulators
783  @{ */
784 
785  /** Simulates a laser range scan into the current grid map.
786  * The simulated scan is stored in a CObservation2DRangeScan object, which
787  *is also used
788  * to pass some parameters: all previously stored characteristics (as
789  *aperture,...) are
790  * taken into account for simulation. Only a few more parameters are
791  *needed. Additive gaussian noise can be optionally added to the simulated
792  *scan.
793  * \param inout_Scan [IN/OUT] This must be filled with desired parameters
794  *before calling, and will contain the scan samples on return.
795  * \param robotPose [IN] The robot pose in this map coordinates. Recall that
796  *sensor pose relative to this robot pose must be specified in the
797  *observation object.
798  * \param threshold [IN] The minimum occupancy threshold to consider a cell
799  *to be occupied (Default: 0.5f)
800  * \param N [IN] The count of range scan "rays", by default to 361.
801  * \param noiseStd [IN] The standard deviation of measurement noise. If not
802  *desired, set to 0.
803  * \param decimation [IN] The rays that will be simulated are at indexes: 0,
804  *D, 2D, 3D, ... Default is D=1
805  * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added
806  *to the angles at which ranges are measured (in radians).
807  *
808  * \sa laserScanSimulatorWithUncertainty(), sonarSimulator(),
809  *COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
810  */
811  void laserScanSimulator(
813  const mrpt::poses::CPose2D& robotPose, float threshold = 0.6f,
814  size_t N = 361, float noiseStd = 0, unsigned int decimation = 1,
815  float angleNoiseStd = mrpt::DEG2RAD(.0)) const;
816 
817  /** Simulates the observations of a sonar rig into the current grid map.
818  * The simulated ranges are stored in a CObservationRange object, which is
819  * also used
820  * to pass in some needed parameters, as the poses of the sonar sensors
821  * onto the mobile robot.
822  * \param inout_observation [IN/OUT] This must be filled with desired
823  * parameters before calling, and will contain the simulated ranges on
824  * return.
825  * \param robotPose [IN] The robot pose in this map coordinates. Recall that
826  * sensor pose relative to this robot pose must be specified in the
827  * observation object.
828  * \param threshold [IN] The minimum occupancy threshold to consider a cell
829  * to be occupied (Default: 0.5f)
830  * \param rangeNoiseStd [IN] The standard deviation of measurement noise. If
831  * not desired, set to 0.
832  * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added
833  * to the angles at which ranges are measured (in radians).
834  *
835  * \sa laserScanSimulator(),
836  * COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
837  */
838  void sonarSimulator(
839  mrpt::obs::CObservationRange& inout_observation,
840  const mrpt::poses::CPose2D& robotPose, float threshold = 0.5f,
841  float rangeNoiseStd = 0.f,
842  float angleNoiseStd = mrpt::DEG2RAD(0.f)) const;
843 
844  /** Simulate just one "ray" in the grid map. This method is used internally
845  * to sonarSimulator and laserScanSimulator. \sa
846  * COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS */
847  void simulateScanRay(
848  const double x, const double y, const double angle_direction,
849  float& out_range, bool& out_valid, const double max_range_meters,
850  const float threshold_free = 0.4f, const double noiseStd = .0,
851  const double angleNoiseStd = .0) const;
852 
853  /** Methods for TLaserSimulUncertaintyParams in
854  * laserScanSimulatorWithUncertainty() */
856  {
857  /** Performs an unscented transform */
859  /** Montecarlo-based estimation */
861  };
862 
863  /** Input params for laserScanSimulatorWithUncertainty() */
865  {
866  /** (Default: sumMonteCarlo) Select the method to do the uncertainty
867  * propagation */
869  /** @name Parameters for each uncertainty method
870  @{ */
871  /** [sumUnscented] UT parameters. Defaults: alpha=0.99, kappa=0,
872  * betta=2.0 */
874  /** [sumMonteCarlo] MonteCarlo parameter: number of samples (Default:
875  * 10) */
876  size_t MC_samples;
877  /** @} */
878 
879  /** @name Generic parameters for all methods
880  @{ */
881  /** The robot pose Gaussian, in map coordinates. Recall that sensor pose
882  * relative to this robot pose must be specified in the observation
883  * object */
885  /** (Default: M_PI) The "aperture" or field-of-view of the range finder,
886  * in radians (typically M_PI = 180 degrees). */
887  float aperture;
888  /** (Default: true) The scanning direction: true=counterclockwise;
889  * false=clockwise */
891  /** (Default: 80) The maximum range allowed by the device, in meters
892  * (e.g. 80m, 50m,...) */
893  float maxRange;
894  /** (Default: at origin) The 6D pose of the sensor on the robot at the
895  * moment of starting the scan. */
897  size_t nRays;
898  /** (Default: 0) The standard deviation of measurement noise. If not
899  * desired, set to 0 */
901  /** (Default: 0) The sigma of an optional Gaussian noise added to the
902  * angles at which ranges are measured (in radians) */
904  /** (Default: 1) The rays that will be simulated are at indexes: 0, D,
905  * 2D, 3D,... */
906  unsigned int decimation;
907  /** (Default: 0.6f) The minimum occupancy threshold to consider a cell
908  * to be occupied */
909  float threshold;
910  /** @} */
911 
913  };
914 
915  /** Output params for laserScanSimulatorWithUncertainty() */
917  {
918  /** The scan + its uncertainty */
920 
922  };
923 
924  /** Like laserScanSimulatorWithUncertainty() (see it for a discussion of
925  * most parameters) but taking into account
926  * the robot pose uncertainty and generating a vector of predicted
927  * variances for each ray.
928  * Range uncertainty includes both, sensor noise and large non-linear
929  * effects caused by borders and discontinuities in the environment
930  * as seen from different robot poses.
931  *
932  * \param in_params [IN] Input settings. See TLaserSimulUncertaintyParams
933  * \param in_params [OUT] Output range + uncertainty.
934  *
935  * \sa laserScanSimulator(),
936  * COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
937  */
939  const TLaserSimulUncertaintyParams& in_params,
940  TLaserSimulUncertaintyResult& out_results) const;
941 
942  /** @} */
943 
944  /** Computes the likelihood [0,1] of a set of points, given the current grid
945  * map as reference.
946  * \param pm The points map
947  * \param relativePose The relative pose of the points map in this map's
948  * coordinates, or nullptr for (0,0,0).
949  * See "likelihoodOptions" for configuration parameters.
950  */
952  const CPointsMap* pm,
953  const mrpt::poses::CPose2D* relativePose = nullptr);
954 
955  /** Computes the likelihood [0,1] of a set of points, given the current grid
956  * map as reference.
957  * \param pm The points map
958  * \param relativePose The relative pose of the points map in this map's
959  * coordinates, or nullptr for (0,0,0).
960  * See "likelihoodOptions" for configuration parameters.
961  */
963  const CPointsMap* pm,
964  const mrpt::poses::CPose2D* relativePose = nullptr);
965 
966  /** Saves the gridmap as a graphical file (BMP,PNG,...).
967  * The format will be derived from the file extension (see
968  * CImage::saveToFile )
969  * \return False on any error.
970  */
971  bool saveAsBitmapFile(const std::string& file) const;
972 
973  /** Saves a composite image with two gridmaps and lines representing a set
974  * of correspondences between them.
975  * \sa saveAsEMFTwoMapsWithCorrespondences
976  * \return False on any error.
977  */
979  const std::string& fileName, const COccupancyGridMap2D* m1,
980  const COccupancyGridMap2D* m2,
981  const mrpt::tfest::TMatchingPairList& corrs);
982 
983  /** Saves a composite image with two gridmaps and numbers for the
984  * correspondences between them.
985  * \sa saveAsBitmapTwoMapsWithCorrespondences
986  * \return False on any error.
987  */
989  const std::string& fileName, const COccupancyGridMap2D* m1,
990  const COccupancyGridMap2D* m2,
991  const mrpt::tfest::TMatchingPairList& corrs);
992 
993  /** Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel
994  * is 1 cell, and with an overlay of landmarks.
995  * \note The template parameter CLANDMARKSMAP is assumed to be
996  * mrpt::maps::CLandmarksMap normally.
997  * \return False on any error.
998  */
999  template <class CLANDMARKSMAP>
1001  const std::string& file, const CLANDMARKSMAP* landmarks,
1002  bool addTextLabels = false,
1003  const mrpt::img::TColor& marks_color =
1004  mrpt::img::TColor(0, 0, 255)) const
1005  {
1006  MRPT_START
1007  mrpt::img::CImage img(1, 1, 3);
1008  getAsImageFiltered(img, false, true); // in RGB
1009  const bool topleft = img.isOriginTopLeft();
1010  for (unsigned int i = 0; i < landmarks->landmarks.size(); i++)
1011  {
1012  const typename CLANDMARKSMAP::landmark_type* lm =
1013  landmarks->landmarks.get(i);
1014  int px = x2idx(lm->pose_mean.x);
1015  int py = topleft ? size_y - 1 - y2idx(lm->pose_mean.y)
1016  : y2idx(lm->pose_mean.y);
1017  img.rectangle(px - 7, (py + 7), px + 7, (py - 7), marks_color);
1018  img.rectangle(px - 6, (py + 6), px + 6, (py - 6), marks_color);
1019  if (addTextLabels)
1020  img.textOut(
1021  px, py - 8, format("%u", i), mrpt::img::TColor::black());
1022  }
1023  return img.saveToFile(file.c_str());
1024  MRPT_END
1025  }
1026 
1027  /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell
1028  * (output image is RGB only if forceRGB is true)
1029  * If "tricolor" is true, only three gray levels will appear in the image:
1030  * gray for unobserved cells, and black/white for occupied/empty cells
1031  * respectively.
1032  * \sa getAsImageFiltered
1033  */
1034  void getAsImage(
1035  mrpt::img::CImage& img, bool verticalFlip = false,
1036  bool forceRGB = false, bool tricolor = false) const;
1037 
1038  /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell
1039  * (output image is RGB only if forceRGB is true) - This method filters the
1040  * image for easy feature detection
1041  * If "tricolor" is true, only three gray levels will appear in the image:
1042  * gray for unobserved cells, and black/white for occupied/empty cells
1043  * respectively.
1044  * \sa getAsImage
1045  */
1046  void getAsImageFiltered(
1047  img::CImage& img, bool verticalFlip = false,
1048  bool forceRGB = false) const;
1049 
1050  /** Returns a 3D plane with its texture being the occupancy grid and
1051  * transparency proportional to "uncertainty" (i.e. a value of 0.5 is fully
1052  * transparent)
1053  */
1054  void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
1055 
1056  /** Get a point cloud with all (border) occupied cells as points */
1057  void getAsPointCloud(
1059  const float occup_threshold = 0.5f) const;
1060 
1061  /** Returns true upon map construction or after calling clear(), the return
1062  * changes to false upon successful insertObservation() or any other
1063  * method to load data in the map.
1064  */
1065  bool isEmpty() const override;
1066 
1067  /** Load the gridmap from a image in a file (the format can be any supported
1068  * by CImage::loadFromFile).
1069  * \param file The file to be loaded.
1070  * \param resolution The size of a pixel (cell), in meters. Recall cells are
1071  * always squared, so just a dimension is needed.
1072  * \param xCentralPixel The `x` coordinate (0=first, increases <b>left to
1073  * right</b> on the image) for the pixel which will be taken at coordinates
1074  * origin (0,0). (Default: the center of the image)
1075  * \param yCentralPixel The `y` coordinate (0=first, increases <b>BOTTOM
1076  * upwards</b> on the image) for the pixel which will be taken at
1077  * coordinates origin (0,0). (Default: the center of the image)
1078  * \return False on any error.
1079  * \sa loadFromBitmap
1080  */
1081  bool loadFromBitmapFile(
1082  const std::string& file, float resolution, float xCentralPixel = -1,
1083  float yCentralPixel = -1);
1084 
1085  /** Load the gridmap from a image in a file (the format can be any supported
1086  * by CImage::loadFromFile).
1087  * See loadFromBitmapFile() for the meaning of parameters */
1088  bool loadFromBitmap(
1089  const mrpt::img::CImage& img, float resolution,
1090  float xCentralPixel = -1, float yCentralPixel = -1);
1091 
1092  /** See the base class for more details: In this class it is implemented as
1093  * correspondences of the passed points map to occupied cells.
1094  * NOTICE: That the "z" dimension is ignored in the points. Clip the points
1095  * as appropiated if needed before calling this method.
1096  *
1097  * \sa computeMatching3DWith
1098  */
1099  virtual void determineMatching2D(
1100  const mrpt::maps::CMetricMap* otherMap,
1101  const mrpt::poses::CPose2D& otherMapPose,
1102  mrpt::tfest::TMatchingPairList& correspondences,
1103  const TMatchingParams& params,
1104  TMatchingExtraResults& extraResults) const override;
1105 
1106  /** See docs in base class: in this class this always returns 0 */
1107  float compute3DMatchingRatio(
1108  const mrpt::maps::CMetricMap* otherMap,
1109  const mrpt::poses::CPose3D& otherMapPose,
1110  const TMatchingRatioParams& params) const override;
1111 
1112  /** This virtual method saves the map to a file "filNamePrefix"+<
1113  * some_file_extension >, as an image or in any other applicable way (Notice
1114  * that other methods to save the map may be implemented in classes
1115  * implementing this virtual interface). */
1117  const std::string& filNamePrefix) const override;
1118 
1119  /** The structure used to store the set of Voronoi diagram
1120  * critical points.
1121  * \sa findCriticalPoints
1122  */
1124  {
1126  : x(),
1127  y(),
1128  clearance(),
1129  x_basis1(),
1130  y_basis1(),
1131  x_basis2(),
1132  y_basis2()
1133  {
1134  }
1135 
1136  /** The coordinates of critical point. */
1137  std::vector<int> x, y;
1138  /** The clearance of critical points, in 1/100 of cells. */
1139  std::vector<int> clearance;
1140  /** Their two first basis points coordinates. */
1141  std::vector<int> x_basis1, y_basis1, x_basis2, y_basis2;
1143 
1144  private:
1145  // See docs in base class
1147  const mrpt::obs::CObservation* obs,
1148  const mrpt::poses::CPose3D& takenFrom) override;
1149  // See docs in base class
1151  const mrpt::obs::CObservation* obs) const override;
1152 
1153  /** Returns a byte with the occupancy of the 8 sorrounding cells.
1154  * \param cx The cell index
1155  * \param cy The cell index
1156  * \sa direction2idx
1157  */
1158  inline unsigned char GetNeighborhood(int cx, int cy) const;
1159 
1160  /** Used to store the 8 possible movements from a cell to the
1161  * sorrounding ones.Filled in the constructor.
1162  * \sa direction2idx
1163  */
1165 
1166  /** Returns the index [0,7] of the given movement, or
1167  * -1 if invalid one.
1168  * \sa direccion_vecino_x,direccion_vecino_y,GetNeighborhood
1169  */
1170  int direction2idx(int dx, int dy);
1171 
1173  /** See COccupancyGridMap2D::COccupancyGridMap2D */
1174  float min_x, max_x, min_y, max_y, resolution;
1175  /** Observations insertion options */
1177  /** Probabilistic observation likelihood options */
1180 };
1181 
1182 bool operator<(
1185 } // namespace maps
1186 } // namespace mrpt
1187 
mrpt::poses::CPosePDFGaussian robotPose
The robot pose Gaussian, in map coordinates.
double computeObservationLikelihood_ConsensusOWA(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
void findCriticalPoints(float filter_distance)
Builds a list with the critical points from Voronoi diagram, which must must be built before calling ...
void laserScanSimulator(mrpt::obs::CObservation2DRangeScan &inout_Scan, const mrpt::poses::CPose2D &robotPose, float threshold=0.6f, size_t N=361, float noiseStd=0, unsigned int decimation=1, float angleNoiseStd=mrpt::DEG2RAD(.0)) const
Simulates a laser range scan into the current grid map.
float getXMax() const
Returns the "x" coordinate of right side of grid map.
float consensus_pow
[Consensus] The power factor for the likelihood (default=5)
double computeObservationLikelihood_Consensus(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood" (This method ...
COccupancyGridMap2D(float min_x=-20.0f, float max_x=20.0f, float min_y=-20.0f, float max_y=20.0f, float resolution=0.05f)
Constructor.
void setCell(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
Parameters for CMetricMap::compute3DMatchingRatio()
double getArea() const
Returns the area of the gridmap, in square meters.
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
float getYMin() const
Returns the "y" coordinate of top side of grid map.
int32_t consensus_takeEachRange
[Consensus] The down-sample ratio of ranges (default=1, consider all the ranges)
uint16_t getVoroniClearance(int cx, int cy) const
Reads a the clearance of a cell (in centimeters), after building the Voronoi diagram with buildVorono...
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
#define MRPT_START
Definition: exceptions.h:262
float threshold
(Default: 0.6f) The minimum occupancy threshold to consider a cell to be occupied ...
bool wideningBeamsWithDistance
Enabled: Rays widen with distance to approximate the real behavior of lasers, disabled: insert rays a...
unsigned __int16 uint16_t
Definition: rptypes.h:44
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.
double computeObservationLikelihood_rayTracing(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
std::vector< int > x
The coordinates of critical point.
float getResolution() const
Returns the resolution of the grid map.
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation *obs) const override
Returns true if this map is able to compute a sensible likelihood function for this observation (i...
With this struct options are provided to the observation insertion process.
A 2D range scan plus an uncertainty model for each range.
mrpt::obs::CObservation2DRangeScanWithUncertainty scanWithUncert
The scan + its uncertainty.
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...
const std::vector< cellType > & getRawMap() const
Read-only access to the raw cell contents (cells are in log-odd units)
uint16_t decimation
Specify the decimation of the range scan (default=1 : take all the range values!) ...
mrpt::poses::CPose3D sensorPose
(Default: at origin) The 6D pose of the sensor on the robot at the moment of starting the scan...
double effectiveMappedArea
The target variable for the area of cells with information, i.e.
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
std::vector< float > OWA_weights
[OWA] The sequence of weights to be multiplied to of the ordered list of likelihood values (first one...
double DEG2RAD(const double x)
Degrees to radians.
static double H(double p)
Entropy computation internal function:
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
Output params for laserScanSimulatorWithUncertainty()
mrpt::containers::CDynamicGrid< uint16_t > m_voronoi_diagram
Used to store the Voronoi diagram.
int16_t cellType
The type of the map cells:
float rangeNoiseStd
(Default: 0) The standard deviation of measurement noise.
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.
int y2idx(float y, float ymin) const
void updateCell(int x, int y, float v)
Performs the Bayesian fusion of a new observation of a cell.
mrpt::containers::CDynamicGrid< uint8_t > m_basis_map
Used for Voronoi calculation.Same struct as "map", but contains a "0" if not a basis point...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
struct mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly updateInfoChangeOnly
bool LF_alternateAverageMethod
[LikelihoodField] Set this to "true" ot use an alternative method, where the likelihood of the whole ...
signed char int8_t
Definition: rptypes.h:40
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
void getAsImageFiltered(img::CImage &img, bool verticalFlip=false, bool forceRGB=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
static bool saveAsEMFTwoMapsWithCorrespondences(const std::string &fileName, const COccupancyGridMap2D *m1, const COccupancyGridMap2D *m2, const mrpt::tfest::TMatchingPairList &corrs)
Saves a composite image with two gridmaps and numbers for the correspondences between them...
static const cellType OCCGRID_CELLTYPE_MAX
bool rightToLeft
(Default: true) The scanning direction: true=counterclockwise; false=clockwise
void buildVoronoiDiagram(float threshold, float robot_size, int x1=0, int x2=0, int y1=0, int y2=0)
Build the Voronoi diagram of the grid map.
void freeMap()
Frees the dynamic memory buffers of map.
std::vector< double > precomputedLikelihood
Auxiliary variables to speed up the computation of observation likelihood values for LF method among ...
float MI_ratio_max_distance
[MI] The ratio for the max.
bool rayTracing_useDistanceFilter
[rayTracing] If true (default), the rayTracing method will ignore measured ranges shorter than the si...
std::vector< cellType > map
Store of cell occupancy values.
static bool saveAsBitmapTwoMapsWithCorrespondences(const std::string &fileName, const COccupancyGridMap2D *m1, const COccupancyGridMap2D *m2, const mrpt::tfest::TMatchingPairList &corrs)
Saves a composite image with two gridmaps and lines representing a set of correspondences between the...
const cellType * getRow(int cy) const
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally...
const mrpt::containers::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.
unsigned char uint8_t
Definition: rptypes.h:41
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)
T square(const T x)
Inline function for the square of a number.
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)...
float getCell_nocheck(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
bool saveAsBitmapFile(const std::string &file) const
Saves the gridmap as a graphical file (BMP,PNG,...).
static CLogOddsGridMapLUT< cellType > & get_logodd_lut()
Lookup tables for log-odds.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
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)) }
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap2D, lmMeanInformation)
mrpt::img::CImage CImage
Definition: utils/CImage.h:7
This class allows loading and storing values and vectors of different types from a configuration text...
virtual void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation *) override
See base class.
double computeObservationLikelihood_CellsDifference(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
__int16 int16_t
Definition: rptypes.h:43
float getXMin() const
Returns the "x" coordinate of left side of grid map.
size_t MC_samples
[sumMonteCarlo] MonteCarlo parameter: number of samples (Default: 10)
With this struct options are provided to the observation likelihood computation process.
bool isStaticCell(int cx, int cy, float threshold=0.7f) const
bool isEmpty() const override
Returns true upon map construction or after calling clear(), the return changes to false upon success...
std::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
GLint GLvoid * img
Definition: glext.h:3763
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
static constexpr TColor black()
Definition: TColor.h:65
std::vector< int > x_basis1
Their two first basis points coordinates.
bool loadFromBitmap(const mrpt::img::CImage &img, float resolution, float xCentralPixel=-1, float yCentralPixel=-1)
Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
bool loadFromBitmapFile(const std::string &file, float resolution, float xCentralPixel=-1, float yCentralPixel=-1)
Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
float rayTracing_stdHit
[rayTracing] The laser range sigma.
TLaserSimulUncertaintyMethod
Methods for TLaserSimulUncertaintyParams in laserScanSimulatorWithUncertainty()
void copyMapContentFrom(const COccupancyGridMap2D &otherMap)
copy the gridmap contents, but not all the options, from another map instance
float aperture
(Default: M_PI) The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180...
A list of TMatchingPair.
Definition: TMatchingPair.h:83
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
void subSample(int downRatio)
Performs a downsampling of the gridmap, by a given factor: resolution/=ratio.
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
double I_change
The cummulative change in Information: This is updated only from the "updateCell" method...
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
See the base class for more details: In this class it is implemented as correspondences of the passed...
TUpdateCellsInfoChangeOnly(bool enabled_=false, double I_change_=0, int cellsUpdated_=0)
unsigned char getBasisCell(int x, int y) const
Reads a cell in the "basis" maps.Used for Voronoi calculation.
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
Definition: CDynamicGrid.h:232
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:74
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:252
float maxDistanceInsertion
The largest distance at which cells will be updated (Default 15 meters)
unsigned int decimation
(Default: 1) The rays that will be simulated are at indexes: 0, D, 2D, 3D,...
void sonarSimulator(mrpt::obs::CObservationRange &inout_observation, const mrpt::poses::CPose2D &robotPose, float threshold=0.5f, float rangeNoiseStd=0.f, float angleNoiseStd=mrpt::DEG2RAD(0.f)) const
Simulates the observations of a sonar rig into the current grid map.
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2) ...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOptions
void setCell_nocheck(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
double computeLikelihoodField_II(const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose=nullptr)
Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
static const cellType OCCGRID_P2LTABLE_SIZE
void getAsPointCloud(mrpt::maps::CSimplePointsMap &pm, const float occup_threshold=0.5f) const
Get a point cloud with all (border) occupied cells as points.
double mean_H
The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells) ...
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:254
void laserScanSimulatorWithUncertainty(const TLaserSimulUncertaintyParams &in_params, TLaserSimulUncertaintyResult &out_results) const
Like laserScanSimulatorWithUncertainty() (see it for a discussion of most parameters) but taking into...
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.
An internal structure for storing data related to counting the new information apported by some obser...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
A class for storing an occupancy grid map.
void getAsImage(mrpt::img::CImage &img, bool verticalFlip=false, bool forceRGB=false, bool tricolor=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
float angleNoiseStd
(Default: 0) The sigma of an optional Gaussian noise added to the angles at which ranges are measured...
__int32 int32_t
Definition: rptypes.h:46
int direccion_vecino_x[8]
Used to store the 8 possible movements from a cell to the sorrounding ones.Filled in the constructor...
static const cellType OCCGRID_CELLTYPE_MIN
Discrete to float conversion factors: The min/max values of the integer cell type, eg.
const GLdouble * v
Definition: glext.h:3678
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.
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...
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D plane with its texture being the occupancy grid and transparency proportional to "uncert...
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
int direction2idx(int dx, int dy)
Returns the index [0,7] of the given movement, or -1 if invalid one.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
Input params for laserScanSimulatorWithUncertainty()
void simulateScanRay(const double x, const double y, const double angle_direction, float &out_range, bool &out_valid, const double max_range_meters, const float threshold_free=0.4f, const double noiseStd=.0, const double angleNoiseStd=.0) const
Simulate just one "ray" in the grid map.
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.
Definition: CObservation.h:43
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
const mrpt::containers::CDynamicGrid< uint8_t > & getBasisMap() const
Return the auxiliary "basis" map built while building 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...
static std::vector< float > entropyTable
Internally used to speed-up entropy calculation.
float maxFreenessUpdateCertainty
A value in the range [0.5,1] for updating a free cell.
unsigned char GetNeighborhood(int cx, int cy) const
Returns a byte with the occupancy of the 8 sorrounding cells.
#define MRPT_END
Definition: exceptions.h:266
TLaserSimulUncertaintyMethod method
(Default: sumMonteCarlo) Select the method to do the uncertainty propagation
bool m_is_empty
True upon construction; used by isEmpty()
bool useMapAltitude
The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true...
virtual void internal_clear() override
Clear the map: It set all cells to their default occupancy value (0.5), without changing the resoluti...
virtual bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Insert the observation information into this map.
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 ...
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
float mapAltitude
The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map! ...
float idx2y(const size_t cy) const
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
struct mrpt::maps::COccupancyGridMap2D::TCriticalPointsList CriticalPointsList
static double RAYTRACE_STEP_SIZE_IN_CELL_UNITS
(Default:1.0) Can be set to <1 if a more fine raytracing is needed in sonarSimulator() and laserScanS...
GLenum GLint GLint y
Definition: glext.h:3538
void computeEntropy(TEntropyInfo &info) const
Computes the entropy and related values of this grid map.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
See docs in base class: in this class this always returns 0.
uint32_t size_x
The size of the grid in cells.
void fill(float default_value=0.5f)
Fills all the cells with a default value.
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) noexcept
Change the size of gridmap, maintaining previous contents.
std::vector< TPairLikelihoodIndex > OWA_pairList
[OWA method] This will contain the ascending-ordered list of pairs:(likelihood values, 2D point in map coordinates).
A generic provider of log-odds grid-map maintainance functions.
float x_min
The limits of the grid in "units" (meters)
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
GLsizei const GLfloat * value
Definition: glext.h:4117
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:177
float maxRange
(Default: 80) The maximum range allowed by the device, in meters (e.g.
void setSize(float x_min, float x_max, float y_min, float y_max, float resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0...
A RGB color - 8bit.
Definition: TColor.h:22
float voroni_free_threshold
The free-cells threshold used to compute the Voronoi diagram.
GLenum GLint x
Definition: glext.h:3538
float computePathCost(float x1, float y1, float x2, float y2) const
Compute the &#39;cost&#39; of traversing a segment of the map according to the occupancy of traversed cells...
This class stores any customizable set of metric maps.
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
int x2idx(float x, float xmin) const
Transform a coordinate value into a cell index, using a diferent "x_min" value.
class mrpt::maps::COccupancyGridMap2D::TLikelihoodOutput likelihoodOutputs
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:58
float maxFreenessInvalidRanges
Like maxFreenessUpdateCertainty, but for invalid ranges (no echo).
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.
int computeClearance(int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint=false) const
Compute the clearance of a given cell, and returns its two first basis (closest obstacle) points...
double computeObservationLikelihood_likelihoodField_Thrun(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
unsigned __int32 uint32_t
Definition: rptypes.h:47
double computeLikelihoodField_Thrun(const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose=nullptr)
Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
#define MAP_DEFINITION_END(_CLASS_NAME_)
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.
bool considerInvalidRangesAsFreeSpace
If set to true (default), invalid range values (no echo rays) as consider as free space until "maxOcc...
GLfloat GLfloat p
Definition: glext.h:6305
std::vector< int > clearance
The clearance of critical points, in 1/100 of cells.
GLenum const GLfloat * params
Definition: glext.h:3534
int x2idx(float x) const
Transform a coordinate value into a cell index.
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)...
void setPos(float x, float y, float value)
Change the contents [0,1] of a cell, given its coordinates.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
double computeObservationLikelihood_likelihoodField_II(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
static float l2p(const cellType l)
Scales an integer representation of the log-odd into a real valued probability in [0...
double computeObservationLikelihood_MI(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
bool saveAsBitmapFileWithLandmarks(const std::string &file, const CLANDMARKSMAP *landmarks, bool addTextLabels=false, const mrpt::img::TColor &marks_color=mrpt::img::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.
bool enabled
If set to false (default), this struct is not used.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019