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