Main MRPT website > C++ reference for MRPT 1.5.7
maps/COccupancyGridMap2D.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #ifndef COccupancyGridMap2D_H
11 #define COccupancyGridMap2D_H
12 
15 #include <mrpt/utils/CImage.h>
17 #include <mrpt/maps/CMetricMap.h>
21 #include <mrpt/poses/poses_frwds.h>
24 #include <mrpt/obs/obs_frwds.h>
25 #include <limits>
26 
27 #include <mrpt/maps/link_pragmas.h>
28 
29 #include <mrpt/config.h>
30 #if (!defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && !defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS)) || (defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS))
31  #error One of OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS or OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS must be defined.
32 #endif
33 
34 namespace mrpt
35 {
36 namespace maps
37 {
38  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( COccupancyGridMap2D, CMetricMap, MAPS_IMPEXP )
39 
40  /** A class for storing an occupancy grid map.
41  * COccupancyGridMap2D is a class for storing a metric map
42  * representation in the form of a probabilistic occupancy
43  * grid map: value of 0 means certainly occupied, 1 means
44  * a certainly empty cell. Initially 0.5 means uncertainty.
45  *
46  * The cells keep the log-odd representation of probabilities instead of the probabilities themselves.
47  * More details can be found at http://www.mrpt.org/Occupancy_Grids
48  *
49  * The algorithm for updating the grid from a laser scanner can optionally take into account the progressive widening of the beams, as
50  * described in [this page](http://www.mrpt.org/Occupancy_Grids)
51  *
52  * Some implemented methods are:
53  * - Update of individual cells
54  * - Insertion of observations
55  * - Voronoi diagram and critical points (\a buildVoronoiDiagram)
56  * - Saving and loading from/to a bitmap
57  * - Laser scans simulation for the map contents
58  * - Entropy and information methods (See computeEntropy)
59  *
60  * \ingroup mrpt_maps_grp
61  **/
63  public CMetricMap,
64  // Inherit from the corresponding specialization of CLogOddsGridMap2D<>:
65 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
67 #else
69 #endif
70  {
71  // This must be added to any CSerializable derived class:
73  public:
74 
75  /** The type of the map cells: */
76 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
77  typedef int8_t cellType;
78  typedef uint8_t cellTypeUnsigned;
79 #else
80  typedef int16_t cellType;
82 #endif
83 
84  /** Discrete to float conversion factors: The min/max values of the integer cell type, eg.[0,255] or [0,65535] */
85  static const cellType OCCGRID_CELLTYPE_MIN = CLogOddsGridMap2D<cellType>::CELLTYPE_MIN;
86  static const cellType OCCGRID_CELLTYPE_MAX = CLogOddsGridMap2D<cellType>::CELLTYPE_MAX;
87  static const cellType OCCGRID_P2LTABLE_SIZE = CLogOddsGridMap2D<cellType>::P2LTABLE_SIZE;
88 
89  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 laserScanSimulator(), or >1 to speed it up.
90 
91  protected:
92 
93  friend class CMultiMetricMap;
94  friend class CMultiMetricMapPDF;
95 
96  void freeMap(); //!< Frees the dynamic memory buffers of map.
97  static CLogOddsGridMapLUT<cellType> m_logodd_lut; //!< Lookup tables for log-odds
98 
99  std::vector<cellType> map; //!< Store of cell occupancy values. Order: row by row, from left to right
100  uint32_t size_x,size_y; //!< The size of the grid in cells
101  float x_min,x_max,y_min,y_max; //!< The limits of the grid in "units" (meters)
102  float resolution; //!< Cell size, i.e. resolution of the grid map.
103 
104  std::vector<double> precomputedLikelihood; //!< Auxiliary variables to speed up the computation of observation likelihood values for LF method among others, at a high cost in memory (see TLikelihoodOptions::enableLikelihoodCache).
106 
107  /** Used for Voronoi calculation.Same struct as "map", but contains a "0" if not a basis point. */
109 
110  /** Used to store the Voronoi diagram.
111  * Contains the distance of each cell to its closer obstacles
112  * in 1/100th distance units (i.e. in centimeters), or 0 if not into the Voronoi diagram */
114 
115  bool m_is_empty; //!< True upon construction; used by isEmpty()
116 
117  virtual void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation *) MRPT_OVERRIDE; //!< See base class
118 
119  float voroni_free_threshold; //!< The free-cells threshold used to compute the Voronoi diagram.
120 
121  static double H(double p); //!< Entropy computation internal function:
122  static std::vector<float> entropyTable; //!< Internally used to speed-up entropy calculation
123 
124  /** Change the contents [0,1] of a cell, given its index */
125  inline void setCell_nocheck(int x,int y,float value) {
126  map[x+y*size_x]=p2l(value);
127  }
128 
129  /** Read the real valued [0,1] contents of a cell, given its index */
130  inline float getCell_nocheck(int x,int y) const {
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  if (cellIndex<size_x*size_y)
136  map[cellIndex] = b;
137  }
138 
139  /** One of the methods that can be selected for implementing "computeObservationLikelihood" (This method is the Range-Scan Likelihood Consensus for gridmaps, see the ICRA2007 paper by Blanco et al.) */
140  double computeObservationLikelihood_Consensus(const mrpt::obs::CObservation *obs,const mrpt::poses::CPose2D &takenFrom );
141  /** One of the methods that can be selected for implementing "computeObservationLikelihood". TODO: This method is described in.... */
142  double computeObservationLikelihood_ConsensusOWA(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom );
143  /** One of the methods that can be selected for implementing "computeObservationLikelihood" */
144  double computeObservationLikelihood_CellsDifference(const mrpt::obs::CObservation *obs,const mrpt::poses::CPose2D &takenFrom );
145  /** One of the methods that can be selected for implementing "computeObservationLikelihood" */
146  double computeObservationLikelihood_MI(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom );
147  /** One of the methods that can be selected for implementing "computeObservationLikelihood" */
148  double computeObservationLikelihood_rayTracing(const mrpt::obs::CObservation *obs,const mrpt::poses::CPose2D &takenFrom );
149  /** One of the methods that can be selected for implementing "computeObservationLikelihood".*/
150  double computeObservationLikelihood_likelihoodField_Thrun(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom );
151  /** One of the methods that can be selected for implementing "computeObservationLikelihood". */
152  double computeObservationLikelihood_likelihoodField_II(const mrpt::obs::CObservation *obs,const mrpt::poses::CPose2D &takenFrom );
153 
154  virtual void internal_clear( ) MRPT_OVERRIDE; //!< Clear the map: It set all cells to their default occupancy value (0.5), without changing the resolution (the grid extension is reset to the default values).
155 
156  /** Insert the observation information into this map.
157  *
158  * \param obs The observation
159  * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg)
160  *
161  * After successfull execution, "lastObservationInsertionInfo" is updated.
162  *
163  * \sa insertionOptions, CObservation::insertObservationInto
164  */
165  virtual bool internal_insertObservation( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
166 
167  public:
168  /** Read-only access to the raw cell contents (cells are in log-odd units) */
169  const std::vector<cellType> & getRawMap() const { return this->map; }
170  /** Performs the Bayesian fusion of a new observation of a cell \sa updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free */
171  void updateCell(int x,int y, float v);
172 
173  /** An internal structure for storing data related to counting the new information apported by some observation */
175  {
176  TUpdateCellsInfoChangeOnly( bool enabled = false, double I_change = 0, int cellsUpdated=0) : enabled(enabled), I_change(I_change), cellsUpdated(cellsUpdated), laserRaysSkip(1)
177  {
178  }
179  bool enabled; //!< If set to false (default), this struct is not used. Set to true only when measuring the info of an observation.
180  double I_change; //!< The cummulative change in Information: This is updated only from the "updateCell" method.
181  int cellsUpdated; //!< The cummulative updated cells count: This is updated only from the "updateCell" method.
182  int laserRaysSkip; //!< In this mode, some laser rays can be skips to speep-up
183  } updateInfoChangeOnly;
184 
185  void fill(float default_value = 0.5f ); //!< Fills all the cells with a default value.
186 
187  /** Constructor */
188  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 );
189  /** Destructor */
190  virtual ~COccupancyGridMap2D();
191 
192  /** Change the size of gridmap, erasing all its previous contents.
193  * \param x_min The "x" coordinates of left most side of grid.
194  * \param x_max The "x" coordinates of right most side of grid.
195  * \param y_min The "y" coordinates of top most side of grid.
196  * \param y_max The "y" coordinates of bottom most side of grid.
197  * \param resolution The new size of cells.
198  * \param default_value The value of cells, tipically 0.5.
199  * \sa ResizeGrid
200  */
201  void setSize(float x_min,float x_max,float y_min,float y_max,float resolution,float default_value = 0.5f);
202 
203  /** Change the size of gridmap, maintaining previous contents.
204  * \param new_x_min The "x" coordinates of new left most side of grid.
205  * \param new_x_max The "x" coordinates of new right most side of grid.
206  * \param new_y_min The "y" coordinates of new top most side of grid.
207  * \param new_y_max The "y" coordinates of new bottom most side of grid.
208  * \param new_cells_default_value The value of the new cells, tipically 0.5.
209  * \param additionalMargin If set to true (default), an additional margin of a few meters will be added to the grid, ONLY if the new coordinates are larger than current ones.
210  * \sa setSize
211  */
212  void resizeGrid(float new_x_min,float new_x_max,float new_y_min,float new_y_max,float new_cells_default_value = 0.5f, bool additionalMargin = true) MRPT_NO_THROWS;
213 
214  /** Returns the area of the gridmap, in square meters */
215  inline double getArea() const { return size_x*size_y*mrpt::math::square(resolution); }
216 
217  /** Returns the horizontal size of grid map in cells count */
218  inline unsigned int getSizeX() const { return size_x; }
219 
220  /** Returns the vertical size of grid map in cells count */
221  inline unsigned int getSizeY() const { return size_y; }
222 
223  /** Returns the "x" coordinate of left side of grid map */
224  inline float getXMin() const { return x_min; }
225 
226  /** Returns the "x" coordinate of right side of grid map */
227  inline float getXMax() const { return x_max; }
228 
229  /** Returns the "y" coordinate of top side of grid map */
230  inline float getYMin() const { return y_min; }
231 
232  /** Returns the "y" coordinate of bottom side of grid map */
233  inline float getYMax() const { return y_max; }
234 
235  /** Returns the resolution of the grid map */
236  inline float getResolution() const { return resolution; }
237 
238  /** Transform a coordinate value into a cell index */
239  inline int x2idx(float x) const { return static_cast<int>((x-x_min)/resolution ); }
240  inline int y2idx(float y) const { return static_cast<int>((y-y_min)/resolution ); }
241 
242  inline int x2idx(double x) const { return static_cast<int>((x-x_min)/resolution ); }
243  inline int y2idx(double y) const { return static_cast<int>((y-y_min)/resolution ); }
244 
245  /** Transform a cell index into a coordinate value */
246  inline float idx2x(const size_t cx) const { return x_min+(cx+0.5f)*resolution; }
247  inline float idx2y(const size_t cy) const { return y_min+(cy+0.5f)*resolution; }
248 
249  /** Transform a coordinate value into a cell index, using a diferent "x_min" value */
250  inline int x2idx(float x,float x_min) const { return static_cast<int>((x-x_min)/resolution ); }
251  inline int y2idx(float y, float y_min) const { return static_cast<int>((y-y_min)/resolution ); }
252 
253  /** Scales an integer representation of the log-odd into a real valued probability in [0,1], using p=exp(l)/(1+exp(l)) */
254  static inline float l2p(const cellType l) {
255  return m_logodd_lut.l2p(l);
256  }
257  /** Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)) */
258  static inline uint8_t l2p_255(const cellType l) {
259  return m_logodd_lut.l2p_255(l);
260  }
261  /** Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the valid range of cellType */
262  static inline cellType p2l(const float p) {
263  return m_logodd_lut.p2l(p);
264  }
265 
266  /** Change the contents [0,1] of a cell, given its index */
267  inline void setCell(int x,int y,float value)
268  {
269  // The x> comparison implicitly holds if x<0
270  if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
271  return;
272  else map[x+y*size_x]=p2l(value);
273  }
274 
275  /** Read the real valued [0,1] contents of a cell, given its index */
276  inline float getCell(int x,int y) const
277  {
278  // The x> comparison implicitly holds if x<0
279  if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
280  return 0.5f;
281  else return l2p(map[x+y*size_x]);
282  }
283 
284  /** Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally */
285  inline cellType *getRow( int cy ) { if (cy<0 || static_cast<unsigned int>(cy)>=size_y) return NULL; else return &map[0+cy*size_x]; }
286 
287  /** Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally */
288  inline const cellType *getRow( int cy ) const { if (cy<0 || static_cast<unsigned int>(cy)>=size_y) return NULL; else return &map[0+cy*size_x]; }
289 
290  /** Change the contents [0,1] of a cell, given its coordinates */
291  inline void setPos(float x,float y,float value) { setCell(x2idx(x),y2idx(y),value); }
292 
293  /** Read the real valued [0,1] contents of a cell, given its coordinates */
294  inline float getPos(float x,float y) const { return getCell(x2idx(x),y2idx(y)); }
295 
296  /** Returns "true" if cell is "static", i.e.if its occupancy is below a given threshold */
297  inline bool isStaticPos(float x,float y,float threshold = 0.7f) const { return isStaticCell(x2idx(x),y2idx(y),threshold); }
298  inline bool isStaticCell(int cx,int cy,float threshold = 0.7f) const { return (getCell(cx,cy)<=threshold); }
299 
300  /** Change a cell in the "basis" maps.Used for Voronoi calculation */
301  inline void setBasisCell(int x,int y,uint8_t value)
302  {
303  uint8_t *cell=m_basis_map.cellByIndex(x,y);
304 #ifdef _DEBUG
305  ASSERT_ABOVEEQ_(x,0)
306  ASSERT_ABOVEEQ_(y,0)
307  ASSERT_BELOWEQ_(x,int(m_basis_map.getSizeX()))
308  ASSERT_BELOWEQ_(y,int(m_basis_map.getSizeY()))
309 #endif
310  *cell = value;
311  }
312 
313  /** Reads a cell in the "basis" maps.Used for Voronoi calculation */
314  inline unsigned char getBasisCell(int x,int y) const
315  {
316  const uint8_t *cell=m_basis_map.cellByIndex(x,y);
317 #ifdef _DEBUG
318  ASSERT_ABOVEEQ_(x,0)
319  ASSERT_ABOVEEQ_(y,0)
320  ASSERT_BELOWEQ_(x,int(m_basis_map.getSizeX()))
321  ASSERT_BELOWEQ_(y,int(m_basis_map.getSizeY()))
322 #endif
323  return *cell;
324  }
325 
326  void copyMapContentFrom(const COccupancyGridMap2D &otherMap); //!< copy the gridmap contents, but not all the options, from another map instance
327 
328  /** Used for returning entropy related information \sa computeEntropy */
330  {
331  TEntropyInfo() : H(0),I(0),mean_H(0),mean_I(0),effectiveMappedArea(0),effectiveMappedCells(0)
332  {
333  }
334  double H; //!< The target variable for absolute entropy, computed as:<br><center>H(map)=Sum<sub>x,y</sub>{ -p(x,y)*ln(p(x,y)) -(1-p(x,y))*ln(1-p(x,y)) }</center><br><br>
335  double I; //!< The target variable for absolute "information", defining I(x) = 1 - H(x)
336  double mean_H; //!< The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells)
337  double mean_I; //!< The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (cells)
338  double effectiveMappedArea;//!< The target variable for the area of cells with information, i.e. p(x)!=0.5
339  unsigned long effectiveMappedCells; //!< The mapped area in cells.
340  };
341 
342  /** With this struct options are provided to the observation insertion process.
343  * \sa CObservation::insertIntoGridMap */
345  {
346  public:
347  /** Initilization of default parameters
348  */
350 
351  /** This method load the options from a ".ini" file.
352  * Only those parameters found in the given "section" and having
353  * the same name that the variable are loaded. Those not found in
354  * the file will stay with their previous values (usually the default
355  * values loaded at initialization). An example of an ".ini" file:
356  * \code
357  * [section]
358  * resolution=0.10 ; blah blah...
359  * modeSelection=1 ; 0=blah, 1=blah,...
360  * \endcode
361  */
362  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
363  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
364 
365  float mapAltitude; //!< The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map!
366  bool useMapAltitude; //!< The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true.
367  float maxDistanceInsertion; //!< The largest distance at which cells will be updated (Default 15 meters)
368  float maxOccupancyUpdateCertainty; //!< A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0.8)
369  float maxFreenessUpdateCertainty; //!< A value in the range [0.5,1] for updating a free cell. (default=0 means use the same than maxOccupancyUpdateCertainty)
370  float maxFreenessInvalidRanges; //!< Like maxFreenessUpdateCertainty, but for invalid ranges (no echo). (default=0 means same than maxOccupancyUpdateCertainty)
371  bool considerInvalidRangesAsFreeSpace; //!< If set to true (default), invalid range values (no echo rays) as consider as free space until "maxOccupancyUpdateCertainty", but ONLY when the previous and next rays are also an invalid ray.
372  uint16_t decimation; //!< Specify the decimation of the range scan (default=1 : take all the range values!)
373  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).
374  float CFD_features_gaussian_size; //!< Gaussian sigma of the filter used in getAsImageFiltered (for features detection) (Default=1) (0:Disabled)
375  float CFD_features_median_size; //!< Size of the Median filter used in getAsImageFiltered (for features detection) (Default=3) (0:Disabled)
376  bool wideningBeamsWithDistance; //!< Enabled: Rays widen with distance to approximate the real behavior of lasers, disabled: insert rays as simple lines (Default=false)
377  };
378 
379  TInsertionOptions insertionOptions; //!< With this struct options are provided to the observation insertion process \sa CObservation::insertIntoGridMap
380 
381  /** The type for selecting a likelihood computation method */
383  {
384  lmMeanInformation = 0,
390  lmConsensusOWA
391  };
392 
393  /** With this struct options are provided to the observation likelihood computation process */
395  {
396  public:
397  TLikelihoodOptions(); //!< Initilization of default parameters
398 
399  /** This method load the options from a ".ini" file.
400  * Only those parameters found in the given "section" and having
401  * the same name that the variable are loaded. Those not found in
402  * the file will stay with their previous values (usually the default
403  * values loaded at initialization). An example of an ".ini" file:
404  * \code
405  * [section]
406  * resolution=0.10 ; blah blah...
407  * modeSelection=1 ; 0=blah, 1=blah,...
408  * \endcode
409  */
410  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
411  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
412 
413  TLikelihoodMethod likelihoodMethod; //!< The selected method to compute an observation likelihood
414  float LF_stdHit; //!< [LikelihoodField] The laser range "sigma" used in computations; Default value = 0.35
415  float LF_zHit, LF_zRandom; //!< [LikelihoodField]
416  float LF_maxRange; //!< [LikelihoodField] The max. range of the sensor (Default= 81 m)
417  uint32_t LF_decimation; //!< [LikelihoodField] The decimation of the points in a scan, default=1 == no decimation
418  float LF_maxCorrsDistance; //!< [LikelihoodField] The max. distance for searching correspondences around each sensed point
419  bool LF_useSquareDist; //!< [LikelihoodField] (Default:false) Use `exp(dist^2/std^2)` instead of `exp(dist^2/std^2)`
420  bool LF_alternateAverageMethod; //!< [LikelihoodField] Set this to "true" ot use an alternative method, where the likelihood of the whole range scan is computed by "averaging" of individual ranges, instead of by the "product".
421  float MI_exponent; //!< [MI] The exponent in the MI likelihood computation. Default value = 5
422  uint32_t MI_skip_rays; //!< [MI] The scan rays decimation: at every N rays, one will be used to compute the MI
423  float MI_ratio_max_distance; //!< [MI] The ratio for the max. distance used in the MI computation and in the insertion of scans, e.g. if set to 2.0 the MI will use twice the distance that the update distance.
424  bool rayTracing_useDistanceFilter; //!< [rayTracing] If true (default), the rayTracing method will ignore measured ranges shorter than the simulated ones.
425  int32_t rayTracing_decimation; //!< [rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to use all the sensed ranges.
426  float rayTracing_stdHit; //!< [rayTracing] The laser range sigma.
427  int32_t consensus_takeEachRange; //!< [Consensus] The down-sample ratio of ranges (default=1, consider all the ranges)
428  float consensus_pow; //!< [Consensus] The power factor for the likelihood (default=5)
429  std::vector<float> OWA_weights; //!< [OWA] The sequence of weights to be multiplied to of the ordered list of likelihood values (first one is the largest); the size of this vector determines the number of highest likelihood values to fuse.
430 
431  bool enableLikelihoodCache; //!< Enables the usage of a cache of likelihood values (for LF methods), if set to true (default=false).
432  } likelihoodOptions;
433 
434  typedef std::pair<double,mrpt::math::TPoint2D> TPairLikelihoodIndex; //!< Auxiliary private class.
435 
436  /** Some members of this struct will contain intermediate or output data after calling "computeObservationLikelihood" for some likelihood functions */
438  {
439  public:
440  TLikelihoodOutput() : OWA_pairList(), OWA_individualLikValues()
441  {}
442 
443  std::vector<TPairLikelihoodIndex> OWA_pairList; //!< [OWA method] This will contain the ascending-ordered list of pairs:(likelihood values, 2D point in map coordinates).
444  std::vector<double> OWA_individualLikValues; //!< [OWA method] This will contain the ascending-ordered list of likelihood values for individual range measurements in the scan.
445  } likelihoodOutputs;
446 
447  void subSample( int downRatio ); //!< Performs a downsampling of the gridmap, by a given factor: resolution/=ratio
448 
449  /** Computes the entropy and related values of this grid map.
450  * The entropy is computed as the summed entropy of each cell, taking them as discrete random variables following a Bernoulli distribution:
451  * \param info The output information is returned here */
452  void computeEntropy( TEntropyInfo &info ) const;
453 
454  /** @name Voronoi methods
455  @{ */
456 
457  /** Build the Voronoi diagram of the grid map.
458  * \param threshold The threshold for binarizing the map.
459  * \param robot_size Size in "units" (meters) of robot, approx.
460  * \param x1 Left coordinate of area to be computed. Default, entire map.
461  * \param x2 Right coordinate of area to be computed. Default, entire map.
462  * \param y1 Top coordinate of area to be computed. Default, entire map.
463  * \param y2 Bottom coordinate of area to be computed. Default, entire map.
464  * \sa findCriticalPoints
465  */
466  void buildVoronoiDiagram(float threshold, float robot_size,int x1=0,int x2=0, int y1=0,int y2=0);
467 
468  /** Reads a the clearance of a cell (in centimeters), after building the Voronoi diagram with \a buildVoronoiDiagram */
469  inline uint16_t getVoroniClearance(int cx,int cy) const
470  {
471 #ifdef _DEBUG
472  ASSERT_ABOVEEQ_(cx,0)
473  ASSERT_ABOVEEQ_(cy,0)
474  ASSERT_BELOWEQ_(cx,int(m_voronoi_diagram.getSizeX()))
475  ASSERT_BELOWEQ_(cy,int(m_voronoi_diagram.getSizeY()))
476 #endif
477  const uint16_t *cell=m_voronoi_diagram.cellByIndex(cx,cy);
478  return *cell;
479  }
480 
481  protected:
482  /** Used to set the clearance of a cell, while building the Voronoi diagram. */
483  inline void setVoroniClearance(int cx,int cy,uint16_t dist)
484  {
485  uint16_t *cell=m_voronoi_diagram.cellByIndex(cx,cy);
486 #ifdef _DEBUG
487  ASSERT_ABOVEEQ_(cx,0)
488  ASSERT_ABOVEEQ_(cy,0)
489  ASSERT_BELOWEQ_(cx,int(m_voronoi_diagram.getSizeX()))
490  ASSERT_BELOWEQ_(cy,int(m_voronoi_diagram.getSizeY()))
491 #endif
492  *cell = dist;
493  }
494 
495  public:
496 
497  /** Return the auxiliary "basis" map built while building the Voronoi diagram \sa buildVoronoiDiagram */
498  inline const mrpt::utils::CDynamicGrid<uint8_t> & getBasisMap() const { return m_basis_map; }
499 
500  /** Return the Voronoi diagram; each cell contains the distance to its closer obstacle, or 0 if not part of the Voronoi diagram \sa buildVoronoiDiagram */
501  inline const mrpt::utils::CDynamicGrid<uint16_t> & getVoronoiDiagram() const { return m_voronoi_diagram; }
502 
503  /** Builds a list with the critical points from Voronoi diagram, which must
504  * must be built before calling this method.
505  * \param filter_distance The minimum distance between two critical points.
506  * \sa buildVoronoiDiagram
507  */
508  void findCriticalPoints( float filter_distance );
509 
510  /** @} */ // End of Voronoi methods
511 
512 
513  /** Compute the clearance of a given cell, and returns its two first
514  * basis (closest obstacle) points.Used to build Voronoi and critical points.
515  * \return The clearance of the cell, in 1/100 of "cell".
516  * \param cx The cell index
517  * \param cy The cell index
518  * \param basis_x Target buffer for coordinates of basis, having a size of two "ints".
519  * \param basis_y Target buffer for coordinates of basis, having a size of two "ints".
520  * \param nBasis The number of found basis: Can be 0,1 or 2.
521  * \param GetContourPoint If "true" the basis are not returned, but the closest free cells.Default at false.
522  * \sa Build_VoronoiDiagram
523  */
524  int computeClearance( int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint = false ) const;
525 
526  /** An alternative method for computing the clearance of a given location (in meters).
527  * \return The clearance (distance to closest OCCUPIED cell), in meters.
528  */
529  float computeClearance( float x, float y, float maxSearchDistance ) const;
530 
531  /** Compute the 'cost' of traversing a segment of the map according to the occupancy of traversed cells.
532  * \return This returns '1-mean(traversed cells occupancy)', i.e. 0.5 for unknown cells, 1 for a free path.
533  */
534  float computePathCost( float x1, float y1, float x2, float y2 ) const;
535 
536 
537  /** \name Sensor simulators
538  @{ */
539 
540  /** Simulates a laser range scan into the current grid map.
541  * The simulated scan is stored in a CObservation2DRangeScan object, which is also used
542  * to pass some parameters: all previously stored characteristics (as aperture,...) are
543  * taken into account for simulation. Only a few more parameters are needed. Additive gaussian noise can be optionally added to the simulated scan.
544  * \param inout_Scan [IN/OUT] This must be filled with desired parameters before calling, and will contain the scan samples on return.
545  * \param robotPose [IN] The robot pose in this map coordinates. Recall that sensor pose relative to this robot pose must be specified in the observation object.
546  * \param threshold [IN] The minimum occupancy threshold to consider a cell to be occupied (Default: 0.5f)
547  * \param N [IN] The count of range scan "rays", by default to 361.
548  * \param noiseStd [IN] The standard deviation of measurement noise. If not desired, set to 0.
549  * \param decimation [IN] The rays that will be simulated are at indexes: 0, D, 2D, 3D, ... Default is D=1
550  * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians).
551  *
552  * \sa laserScanSimulatorWithUncertainty(), sonarSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
553  */
554  void laserScanSimulator(
556  const mrpt::poses::CPose2D &robotPose,
557  float threshold = 0.6f,
558  size_t N = 361,
559  float noiseStd = 0,
560  unsigned int decimation = 1,
561  float angleNoiseStd = mrpt::utils::DEG2RAD(0) ) const;
562 
563  /** Simulates the observations of a sonar rig into the current grid map.
564  * The simulated ranges are stored in a CObservationRange object, which is also used
565  * to pass in some needed parameters, as the poses of the sonar sensors onto the mobile robot.
566  * \param inout_observation [IN/OUT] This must be filled with desired parameters before calling, and will contain the simulated ranges on return.
567  * \param robotPose [IN] The robot pose in this map coordinates. Recall that sensor pose relative to this robot pose must be specified in the observation object.
568  * \param threshold [IN] The minimum occupancy threshold to consider a cell to be occupied (Default: 0.5f)
569  * \param rangeNoiseStd [IN] The standard deviation of measurement noise. If not desired, set to 0.
570  * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians).
571  *
572  * \sa laserScanSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
573  */
574  void sonarSimulator(
575  mrpt::obs::CObservationRange &inout_observation,
576  const mrpt::poses::CPose2D &robotPose,
577  float threshold = 0.5f,
578  float rangeNoiseStd = 0.f,
579  float angleNoiseStd = mrpt::utils::DEG2RAD(0.f) ) const;
580 
581  /** Simulate just one "ray" in the grid map. This method is used internally to sonarSimulator and laserScanSimulator. \sa COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS */
582  void simulateScanRay(
583  const double x,const double y,const double angle_direction,
584  float &out_range,bool &out_valid,
585  const double max_range_meters,
586  const float threshold_free=0.4f,
587  const double noiseStd=.0, const double angleNoiseStd=.0 ) const;
588 
589  /** Methods for TLaserSimulUncertaintyParams in laserScanSimulatorWithUncertainty() */
591  sumUnscented = 0, //!< Performs an unscented transform
592  sumMonteCarlo //!< Montecarlo-based estimation
593  };
594 
595  /** Input params for laserScanSimulatorWithUncertainty() */
597  {
598  TLaserSimulUncertaintyMethod method; //!< (Default: sumMonteCarlo) Select the method to do the uncertainty propagation
599  /** @name Parameters for each uncertainty method
600  @{ */
601  double UT_alpha, UT_kappa, UT_beta; //!< [sumUnscented] UT parameters. Defaults: alpha=0.99, kappa=0, betta=2.0
602  size_t MC_samples; //!< [sumMonteCarlo] MonteCarlo parameter: number of samples (Default: 10)
603  /** @} */
604 
605  /** @name Generic parameters for all methods
606  @{ */
607  mrpt::poses::CPosePDFGaussian robotPose; //!< The robot pose Gaussian, in map coordinates. Recall that sensor pose relative to this robot pose must be specified in the observation object
608  float aperture; //!< (Default: M_PI) The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
609  bool rightToLeft; //!< (Default: true) The scanning direction: true=counterclockwise; false=clockwise
610  float maxRange; //!< (Default: 80) The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
611  mrpt::poses::CPose3D sensorPose; //!< (Default: at origin) The 6D pose of the sensor on the robot at the moment of starting the scan.
612  size_t nRays;
613  float rangeNoiseStd; //!< (Default: 0) The standard deviation of measurement noise. If not desired, set to 0
614  float angleNoiseStd; //!< (Default: 0) The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians)
615  unsigned int decimation; //!< (Default: 1) The rays that will be simulated are at indexes: 0, D, 2D, 3D,...
616  float threshold; //!< (Default: 0.6f) The minimum occupancy threshold to consider a cell to be occupied
617  /** @} */
618 
620  };
621 
622  /** Output params for laserScanSimulatorWithUncertainty() */
624  {
626 
628  };
629 
630 
631  /** Like laserScanSimulatorWithUncertainty() (see it for a discussion of most parameters) but taking into account
632  * the robot pose uncertainty and generating a vector of predicted variances for each ray.
633  * Range uncertainty includes both, sensor noise and large non-linear effects caused by borders and discontinuities in the environment
634  * as seen from different robot poses.
635  *
636  * \param in_params [IN] Input settings. See TLaserSimulUncertaintyParams
637  * \param in_params [OUT] Output range + uncertainty.
638  *
639  * \sa laserScanSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
640  */
641  void laserScanSimulatorWithUncertainty(const TLaserSimulUncertaintyParams &in_params, TLaserSimulUncertaintyResult &out_results) const;
642 
643  /** @} */
644 
645  /** Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
646  * \param pm The points map
647  * \param relativePose The relative pose of the points map in this map's coordinates, or NULL for (0,0,0).
648  * See "likelihoodOptions" for configuration parameters.
649  */
650  double computeLikelihoodField_Thrun( const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose = NULL);
651 
652  /** Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
653  * \param pm The points map
654  * \param relativePose The relative pose of the points map in this map's coordinates, or NULL for (0,0,0).
655  * See "likelihoodOptions" for configuration parameters.
656  */
657  double computeLikelihoodField_II( const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose = NULL);
658 
659  /** Saves the gridmap as a graphical file (BMP,PNG,...).
660  * The format will be derived from the file extension (see CImage::saveToFile )
661  * \return False on any error.
662  */
663  bool saveAsBitmapFile(const std::string &file) const;
664 
665  /** Saves a composite image with two gridmaps and lines representing a set of correspondences between them.
666  * \sa saveAsEMFTwoMapsWithCorrespondences
667  * \return False on any error.
668  */
669  static bool saveAsBitmapTwoMapsWithCorrespondences(
670  const std::string &fileName,
671  const COccupancyGridMap2D *m1,
672  const COccupancyGridMap2D *m2,
673  const mrpt::utils::TMatchingPairList &corrs);
674 
675  /** Saves a composite image with two gridmaps and numbers for the correspondences between them.
676  * \sa saveAsBitmapTwoMapsWithCorrespondences
677  * \return False on any error.
678  */
679  static bool saveAsEMFTwoMapsWithCorrespondences(
680  const std::string &fileName,
681  const COccupancyGridMap2D *m1,
682  const COccupancyGridMap2D *m2,
683  const mrpt::utils::TMatchingPairList &corrs);
684 
685  /** Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel is 1 cell, and with an overlay of landmarks.
686  * \note The template parameter CLANDMARKSMAP is assumed to be mrpt::maps::CLandmarksMap normally.
687  * \return False on any error.
688  */
689  template <class CLANDMARKSMAP>
691  const std::string &file,
692  const CLANDMARKSMAP *landmarks,
693  bool addTextLabels = false,
694  const mrpt::utils::TColor &marks_color = mrpt::utils::TColor(0,0,255) ) const
695  {
696  MRPT_START
697  using namespace mrpt::utils;
698  CImage img(1,1,3);
699  getAsImageFiltered( img, false, true ); // in RGB
700  const bool topleft = img.isOriginTopLeft();
701  for (unsigned int i=0;i<landmarks->landmarks.size();i++)
702  {
703  const typename CLANDMARKSMAP::landmark_type *lm = landmarks->landmarks.get( i );
704  int px = x2idx( lm->pose_mean.x );
705  int py = topleft ? size_y-1- y2idx( lm->pose_mean.y ) : y2idx( lm->pose_mean.y );
706  img.rectangle( px - 7, (py + 7), px +7, (py -7), marks_color );
707  img.rectangle( px - 6, (py + 6), px +6, (py -6), marks_color );
708  if (addTextLabels)
709  img.textOut(px,py-8,format("%u",i), TColor::black);
710  }
711  return img.saveToFile(file.c_str() );
712  MRPT_END
713  }
714 
715 
716  /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true)
717  * If "tricolor" is true, only three gray levels will appear in the image: gray for unobserved cells, and black/white for occupied/empty cells respectively.
718  * \sa getAsImageFiltered
719  */
720  void getAsImage( utils::CImage &img, bool verticalFlip = false, bool forceRGB=false, bool tricolor = false) const;
721 
722  /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true) - This method filters the image for easy feature detection
723  * If "tricolor" is true, only three gray levels will appear in the image: gray for unobserved cells, and black/white for occupied/empty cells respectively.
724  * \sa getAsImage
725  */
726  void getAsImageFiltered( utils::CImage &img, bool verticalFlip = false, bool forceRGB=false) const;
727 
728  /** Returns a 3D plane with its texture being the occupancy grid and transparency proportional to "uncertainty" (i.e. a value of 0.5 is fully transparent)
729  */
730  void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE;
731 
732  /** Get a point cloud with all (border) occupied cells as points */
733  void getAsPointCloud( mrpt::maps::CSimplePointsMap &pm, const float occup_threshold = 0.5f ) const;
734 
735  /** Returns true upon map construction or after calling clear(), the return
736  * changes to false upon successful insertObservation() or any other method to load data in the map.
737  */
738  bool isEmpty() const MRPT_OVERRIDE;
739 
740  /** Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
741  * \param file The file to be loaded.
742  * \param resolution The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed.
743  * \param origin_xPixel The `x` coordinate (0=first, increases <b>left to right</b> on the image) for the pixel which will be taken at coordinates origin (0,0). (Default: the center of the image)
744  * \param origin_yPixel The `y` coordinate (0=first, increases <b>BOTTOM upwards</b> on the image) for the pixel which will be taken at coordinates origin (0,0). (Default: the center of the image)
745  * \return False on any error.
746  * \sa loadFromBitmap
747  */
748  bool loadFromBitmapFile(const std::string &file, float resolution, float origin_xPixel = std::numeric_limits<float>::max(), float origin_yPixel = std::numeric_limits<float>::max() );
749 
750  /** Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
751  * See loadFromBitmapFile() for the meaning of parameters */
752  bool loadFromBitmap(const mrpt::utils::CImage &img, float resolution, float origin_xPixel = std::numeric_limits<float>::max(), float origin_yPixel = std::numeric_limits<float>::max() );
753 
754  /** See the base class for more details: In this class it is implemented as correspondences of the passed points map to occupied cells.
755  * NOTICE: That the "z" dimension is ignored in the points. Clip the points as appropiated if needed before calling this method.
756  *
757  * \sa computeMatching3DWith
758  */
759  virtual void determineMatching2D(
760  const mrpt::maps::CMetricMap * otherMap,
761  const mrpt::poses::CPose2D & otherMapPose,
762  mrpt::utils::TMatchingPairList & correspondences,
763  const TMatchingParams & params,
764  TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
765 
766 
767  /** See docs in base class: in this class this always returns 0 */
768  float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
769 
770  /** This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface). */
771  void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE;
772 
773  /** The structure used to store the set of Voronoi diagram
774  * critical points.
775  * \sa findCriticalPoints
776  */
778  {
779  TCriticalPointsList() : x(),y(),clearance(),x_basis1(),y_basis1(),x_basis2(),y_basis2()
780  {}
781 
782  std::vector<int> x,y; //!< The coordinates of critical point.
783  std::vector<int> clearance; //!< The clearance of critical points, in 1/100 of cells.
784  std::vector<int> x_basis1,y_basis1, x_basis2,y_basis2; //!< Their two first basis points coordinates.
785  } CriticalPointsList;
786 
787 
788  private:
789  // See docs in base class
790  double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) MRPT_OVERRIDE;
791  // See docs in base class
792  bool internal_canComputeObservationLikelihood( const mrpt::obs::CObservation *obs ) const MRPT_OVERRIDE;
793 
794  /** Returns a byte with the occupancy of the 8 sorrounding cells.
795  * \param cx The cell index
796  * \param cy The cell index
797  * \sa direction2idx
798  */
799  inline unsigned char GetNeighborhood( int cx, int cy ) const;
800 
801  /** Used to store the 8 possible movements from a cell to the
802  * sorrounding ones.Filled in the constructor.
803  * \sa direction2idx
804  */
805  int direccion_vecino_x[8],direccion_vecino_y[8];
806 
807  /** Returns the index [0,7] of the given movement, or
808  * -1 if invalid one.
809  * \sa direccion_vecino_x,direccion_vecino_y,GetNeighborhood
810  */
811  int direction2idx(int dx, int dy);
812 
813 
815  float min_x,max_x,min_y,max_y,resolution; //!< See COccupancyGridMap2D::COccupancyGridMap2D
816  mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts; //!< Observations insertion options
817  mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts; //!< Probabilistic observation likelihood options
819 
820  };
822 
823 
825 
826  } // End of namespace
827 } // End of namespace
828 
829 #endif
mrpt::poses::CPosePDFGaussian robotPose
The robot pose Gaussian, in map coordinates. Recall that sensor pose relative to this robot pose must...
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)
void setCell(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
Parameters for CMetricMap::compute3DMatchingRatio()
bool saveAsBitmapFileWithLandmarks(const std::string &file, const CLANDMARKSMAP *landmarks, bool addTextLabels=false, const mrpt::utils::TColor &marks_color=mrpt::utils::TColor(0, 0, 255)) const
Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel is 1 cell, and with an overlay of landmarks.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
#define ASSERT_BELOWEQ_(__A, __B)
std::vector< int > y_basis2
Their two first basis points coordinates.
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.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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...
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
unsigned __int16 uint16_t
Definition: rptypes.h:46
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. Default value = 5
EIGEN_STRONG_INLINE void fill(const Scalar v)
Definition: eigen_plugins.h:38
float getResolution() const
Returns the resolution of the grid map.
double DEG2RAD(const double x)
Degrees to radians.
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...
int y2idx(float y, float y_min) const
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...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
mrpt::utils::CDynamicGrid< uint8_t > m_basis_map
Used for Voronoi calculation.Same struct as "map", but contains a "0" if not a basis point...
double effectiveMappedArea
The target variable for the area of cells with information, i.e. p(x)!=0.5.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
std::vector< float > OWA_weights
[OWA] The sequence of weights to be multiplied to of the ordered list of likelihood values (first one...
float resolution
Cell size, i.e. resolution of the grid map.
Output params for laserScanSimulatorWithUncertainty()
float rangeNoiseStd
(Default: 0) The standard deviation of measurement noise. If not desired, set to 0 ...
TLikelihoodMethod
The type for selecting a likelihood computation method.
float CFD_features_gaussian_size
Gaussian sigma of the filter used in getAsImageFiltered (for features detection) (Default=1) (0:Disab...
The structure used to store the set of Voronoi diagram critical points.
#define MRPT_NO_THROWS
C++11 noexcept: Used after member declarations.
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:223
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
bool LF_alternateAverageMethod
[LikelihoodField] Set this to "true" ot use an alternative method, where the likelihood of the whole ...
signed char int8_t
Definition: rptypes.h:42
STL namespace.
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
bool rightToLeft
(Default: true) The scanning direction: true=counterclockwise; false=clockwise
const mrpt::utils::CDynamicGrid< uint16_t > & getVoronoiDiagram() const
Return the Voronoi diagram; each cell contains the distance to its closer obstacle, or 0 if not part of the Voronoi diagram.
std::vector< double > precomputedLikelihood
Auxiliary variables to speed up the computation of observation likelihood values for LF method among ...
float MI_ratio_max_distance
[MI] The ratio for the max. distance used in the MI computation and in the insertion of scans...
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. Order: row by row, from left to right.
const cellType * getRow(int cy) const
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally...
This class allows loading and storing values and vectors of different types from a configuration text...
unsigned char uint8_t
Definition: rptypes.h:43
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.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
double H
The target variable for absolute entropy, computed as: H(map)=Sumx,y{ -p(x,y)*ln(p(x,y)) -(1-p(x,y))*ln(1-p(x,y)) }
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
cell_t p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
float LF_maxCorrsDistance
[LikelihoodField] The max. distance for searching correspondences around each sensed point ...
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
__int16 int16_t
Definition: rptypes.h:45
#define MRPT_END
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
const mrpt::utils::CDynamicGrid< uint8_t > & getBasisMap() const
Return the auxiliary "basis" map built while building the Voronoi diagram.
GLint GLvoid * img
Definition: glext.h:3645
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
float rayTracing_stdHit
[rayTracing] The laser range sigma.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
TLaserSimulUncertaintyMethod
Methods for TLaserSimulUncertaintyParams in laserScanSimulatorWithUncertainty()
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:220
float aperture
(Default: M_PI) The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180...
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
TUpdateCellsInfoChangeOnly(bool enabled=false, double I_change=0, int cellsUpdated=0)
A RGB color - 8bit.
Definition: TColor.h:26
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLubyte GLubyte b
Definition: glext.h:5575
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
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...
unsigned char getBasisCell(int x, int y) const
Reads a cell in the "basis" maps.Used for Voronoi calculation.
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,...
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2) ...
GLsizei const GLchar ** string
Definition: glext.h:3919
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the ma...
Definition: CDynamicGrid.h:203
void setCell_nocheck(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
double mean_H
The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells) ...
static CLogOddsGridMapLUT< cellType > m_logodd_lut
Lookup tables for log-odds.
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...
A class for storing an occupancy grid map.
float angleNoiseStd
(Default: 0) The sigma of an optional Gaussian noise added to the angles at which ranges are measured...
#define MRPT_START
__int32 int32_t
Definition: rptypes.h:48
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void setBasisCell(int x, int y, uint8_t value)
Change a cell in the "basis" maps.Used for Voronoi calculation.
#define ASSERT_ABOVEEQ_(__A, __B)
uint8_t l2p_255(const cell_t l)
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)...
unsigned long effectiveMappedCells
The mapped area in cells.
static cellType p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Input params for laserScanSimulatorWithUncertainty()
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.
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
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.
T square(const T x)
Inline function for the square of a number.
backing_store_ptr info
Definition: jmemsys.h:170
float maxFreenessUpdateCertainty
A value in the range [0.5,1] for updating a free cell. (default=0 means use the same than maxOccupanc...
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...
int x2idx(float x, float x_min) const
Transform a coordinate value into a cell index, using a diferent "x_min" value.
void setVoroniClearance(int cx, int cy, uint16_t dist)
Used to set the clearance of a cell, while building the Voronoi diagram.
uint32_t size_y
The size of the grid in cells.
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:3908
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
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
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:3516
std::vector< TPairLikelihoodIndex > OWA_pairList
[OWA method] This will contain the ascending-ordered list of pairs:(likelihood values, 2D point in map coordinates).
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:3929
float maxRange
(Default: 80) The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0...
float voroni_free_threshold
The free-cells threshold used to compute the Voronoi diagram.
GLenum GLint x
Definition: glext.h:3516
This class stores any customizable set of metric maps.
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
float maxFreenessInvalidRanges
Like maxFreenessUpdateCertainty, but for invalid ranges (no echo). (default=0 means same than maxOccu...
double mean_I
The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (ce...
Parameters for the determination of matchings between point clouds, etc.
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0...
unsigned __int32 uint32_t
Definition: rptypes.h:49
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
Some members of this struct will contain intermediate or output data after calling "computeObservatio...
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
Used for returning entropy related information.
std::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
bool considerInvalidRangesAsFreeSpace
If set to true (default), invalid range values (no echo rays) as consider as free space until "maxOcc...
GLfloat GLfloat p
Definition: glext.h:5587
std::vector< int > clearance
The clearance of critical points, in 1/100 of cells.
GLenum const GLfloat * params
Definition: glext.h:3514
int16_t cellType
The type of the map cells:
int x2idx(float x) const
Transform a coordinate value into a cell index.
float LF_maxRange
[LikelihoodField] The max. range of the sensor (Default= 81 m)
static uint8_t l2p_255(const cellType l)
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)...
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ...
void setPos(float x, float y, float value)
Change the contents [0,1] of a cell, given its coordinates.
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
static float l2p(const cellType l)
Scales an integer representation of the log-odd into a real valued probability in [0...
bool enabled
If set to false (default), this struct is not used. Set to true only when measuring the info of an ob...
mrpt::utils::CDynamicGrid< uint16_t > m_voronoi_diagram
Used to store the Voronoi diagram.
std::vector< int > y
The coordinates of critical point.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019