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