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