Main MRPT website > C++ reference for MRPT 1.5.6
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  float maxFreenessUpdateCertainty; //!< A value in the range [0.5,1] for updating a free cell. (default=0 means use the same than maxOccupancyUpdateCertainty)
369  float maxFreenessInvalidRanges; //!< Like maxFreenessUpdateCertainty, but for invalid ranges (no echo). (default=0 means same than maxOccupancyUpdateCertainty)
370  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.
371  uint16_t decimation; //!< Specify the decimation of the range scan (default=1 : take all the range values!)
372  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).
373  float CFD_features_gaussian_size; //!< Gaussian sigma of the filter used in getAsImageFiltered (for features detection) (Default=1) (0:Disabled)
374  float CFD_features_median_size; //!< Size of the Median filter used in getAsImageFiltered (for features detection) (Default=3) (0:Disabled)
375  bool wideningBeamsWithDistance; //!< Enabled: Rays widen with distance to approximate the real behavior of lasers, disabled: insert rays as simple lines (Default=false)
376  };
377 
378  TInsertionOptions insertionOptions; //!< With this struct options are provided to the observation insertion process \sa CObservation::insertIntoGridMap
379 
380  /** The type for selecting a likelihood computation method */
382  {
383  lmMeanInformation = 0,
389  lmConsensusOWA
390  };
391 
392  /** With this struct options are provided to the observation likelihood computation process */
394  {
395  public:
396  TLikelihoodOptions(); //!< Initilization of default parameters
397 
398  /** This method load the options from a ".ini" file.
399  * Only those parameters found in the given "section" and having
400  * the same name that the variable are loaded. Those not found in
401  * the file will stay with their previous values (usually the default
402  * values loaded at initialization). An example of an ".ini" file:
403  * \code
404  * [section]
405  * resolution=0.10 ; blah blah...
406  * modeSelection=1 ; 0=blah, 1=blah,...
407  * \endcode
408  */
409  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
410  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
411 
412  TLikelihoodMethod likelihoodMethod; //!< The selected method to compute an observation likelihood
413  float LF_stdHit; //!< [LikelihoodField] The laser range "sigma" used in computations; Default value = 0.35
414  float LF_zHit, LF_zRandom; //!< [LikelihoodField]
415  float LF_maxRange; //!< [LikelihoodField] The max. range of the sensor (Default= 81 m)
416  uint32_t LF_decimation; //!< [LikelihoodField] The decimation of the points in a scan, default=1 == no decimation
417  float LF_maxCorrsDistance; //!< [LikelihoodField] The max. distance for searching correspondences around each sensed point
418  bool LF_useSquareDist; //!< [LikelihoodField] (Default:false) Use `exp(dist^2/std^2)` instead of `exp(dist^2/std^2)`
419  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".
420  float MI_exponent; //!< [MI] The exponent in the MI likelihood computation. Default value = 5
421  uint32_t MI_skip_rays; //!< [MI] The scan rays decimation: at every N rays, one will be used to compute the MI
422  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.
423  bool rayTracing_useDistanceFilter; //!< [rayTracing] If true (default), the rayTracing method will ignore measured ranges shorter than the simulated ones.
424  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.
425  float rayTracing_stdHit; //!< [rayTracing] The laser range sigma.
426  int32_t consensus_takeEachRange; //!< [Consensus] The down-sample ratio of ranges (default=1, consider all the ranges)
427  float consensus_pow; //!< [Consensus] The power factor for the likelihood (default=5)
428  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.
429 
430  bool enableLikelihoodCache; //!< Enables the usage of a cache of likelihood values (for LF methods), if set to true (default=false).
431  } likelihoodOptions;
432 
433  typedef std::pair<double,mrpt::math::TPoint2D> TPairLikelihoodIndex; //!< Auxiliary private class.
434 
435  /** Some members of this struct will contain intermediate or output data after calling "computeObservationLikelihood" for some likelihood functions */
437  {
438  public:
439  TLikelihoodOutput() : OWA_pairList(), OWA_individualLikValues()
440  {}
441 
442  std::vector<TPairLikelihoodIndex> OWA_pairList; //!< [OWA method] This will contain the ascending-ordered list of pairs:(likelihood values, 2D point in map coordinates).
443  std::vector<double> OWA_individualLikValues; //!< [OWA method] This will contain the ascending-ordered list of likelihood values for individual range measurements in the scan.
444  } likelihoodOutputs;
445 
446  void subSample( int downRatio ); //!< Performs a downsampling of the gridmap, by a given factor: resolution/=ratio
447 
448  /** Computes the entropy and related values of this grid map.
449  * The entropy is computed as the summed entropy of each cell, taking them as discrete random variables following a Bernoulli distribution:
450  * \param info The output information is returned here */
451  void computeEntropy( TEntropyInfo &info ) const;
452 
453  /** @name Voronoi methods
454  @{ */
455 
456  /** Build the Voronoi diagram of the grid map.
457  * \param threshold The threshold for binarizing the map.
458  * \param robot_size Size in "units" (meters) of robot, approx.
459  * \param x1 Left coordinate of area to be computed. Default, entire map.
460  * \param x2 Right coordinate of area to be computed. Default, entire map.
461  * \param y1 Top coordinate of area to be computed. Default, entire map.
462  * \param y2 Bottom coordinate of area to be computed. Default, entire map.
463  * \sa findCriticalPoints
464  */
465  void buildVoronoiDiagram(float threshold, float robot_size,int x1=0,int x2=0, int y1=0,int y2=0);
466 
467  /** Reads a the clearance of a cell (in centimeters), after building the Voronoi diagram with \a buildVoronoiDiagram */
468  inline uint16_t getVoroniClearance(int cx,int cy) const
469  {
470 #ifdef _DEBUG
471  ASSERT_ABOVEEQ_(cx,0)
472  ASSERT_ABOVEEQ_(cy,0)
473  ASSERT_BELOWEQ_(cx,int(m_voronoi_diagram.getSizeX()))
474  ASSERT_BELOWEQ_(cy,int(m_voronoi_diagram.getSizeY()))
475 #endif
476  const uint16_t *cell=m_voronoi_diagram.cellByIndex(cx,cy);
477  return *cell;
478  }
479 
480  protected:
481  /** Used to set the clearance of a cell, while building the Voronoi diagram. */
482  inline void setVoroniClearance(int cx,int cy,uint16_t dist)
483  {
484  uint16_t *cell=m_voronoi_diagram.cellByIndex(cx,cy);
485 #ifdef _DEBUG
486  ASSERT_ABOVEEQ_(cx,0)
487  ASSERT_ABOVEEQ_(cy,0)
488  ASSERT_BELOWEQ_(cx,int(m_voronoi_diagram.getSizeX()))
489  ASSERT_BELOWEQ_(cy,int(m_voronoi_diagram.getSizeY()))
490 #endif
491  *cell = dist;
492  }
493 
494  public:
495 
496  /** Return the auxiliary "basis" map built while building the Voronoi diagram \sa buildVoronoiDiagram */
497  inline const mrpt::utils::CDynamicGrid<uint8_t> & getBasisMap() const { return m_basis_map; }
498 
499  /** Return the Voronoi diagram; each cell contains the distance to its closer obstacle, or 0 if not part of the Voronoi diagram \sa buildVoronoiDiagram */
500  inline const mrpt::utils::CDynamicGrid<uint16_t> & getVoronoiDiagram() const { return m_voronoi_diagram; }
501 
502  /** Builds a list with the critical points from Voronoi diagram, which must
503  * must be built before calling this method.
504  * \param filter_distance The minimum distance between two critical points.
505  * \sa buildVoronoiDiagram
506  */
507  void findCriticalPoints( float filter_distance );
508 
509  /** @} */ // End of Voronoi methods
510 
511 
512  /** Compute the clearance of a given cell, and returns its two first
513  * basis (closest obstacle) points.Used to build Voronoi and critical points.
514  * \return The clearance of the cell, in 1/100 of "cell".
515  * \param cx The cell index
516  * \param cy The cell index
517  * \param basis_x Target buffer for coordinates of basis, having a size of two "ints".
518  * \param basis_y Target buffer for coordinates of basis, having a size of two "ints".
519  * \param nBasis The number of found basis: Can be 0,1 or 2.
520  * \param GetContourPoint If "true" the basis are not returned, but the closest free cells.Default at false.
521  * \sa Build_VoronoiDiagram
522  */
523  int computeClearance( int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint = false ) const;
524 
525  /** An alternative method for computing the clearance of a given location (in meters).
526  * \return The clearance (distance to closest OCCUPIED cell), in meters.
527  */
528  float computeClearance( float x, float y, float maxSearchDistance ) const;
529 
530  /** Compute the 'cost' of traversing a segment of the map according to the occupancy of traversed cells.
531  * \return This returns '1-mean(traversed cells occupancy)', i.e. 0.5 for unknown cells, 1 for a free path.
532  */
533  float computePathCost( float x1, float y1, float x2, float y2 ) const;
534 
535 
536  /** \name Sensor simulators
537  @{ */
538 
539  /** Simulates a laser range scan into the current grid map.
540  * The simulated scan is stored in a CObservation2DRangeScan object, which is also used
541  * to pass some parameters: all previously stored characteristics (as aperture,...) are
542  * taken into account for simulation. Only a few more parameters are needed. Additive gaussian noise can be optionally added to the simulated scan.
543  * \param inout_Scan [IN/OUT] This must be filled with desired parameters before calling, and will contain the scan samples on return.
544  * \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.
545  * \param threshold [IN] The minimum occupancy threshold to consider a cell to be occupied (Default: 0.5f)
546  * \param N [IN] The count of range scan "rays", by default to 361.
547  * \param noiseStd [IN] The standard deviation of measurement noise. If not desired, set to 0.
548  * \param decimation [IN] The rays that will be simulated are at indexes: 0, D, 2D, 3D, ... Default is D=1
549  * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians).
550  *
551  * \sa laserScanSimulatorWithUncertainty(), sonarSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
552  */
553  void laserScanSimulator(
555  const mrpt::poses::CPose2D &robotPose,
556  float threshold = 0.6f,
557  size_t N = 361,
558  float noiseStd = 0,
559  unsigned int decimation = 1,
560  float angleNoiseStd = mrpt::utils::DEG2RAD(0) ) const;
561 
562  /** Simulates the observations of a sonar rig into the current grid map.
563  * The simulated ranges are stored in a CObservationRange object, which is also used
564  * to pass in some needed parameters, as the poses of the sonar sensors onto the mobile robot.
565  * \param inout_observation [IN/OUT] This must be filled with desired parameters before calling, and will contain the simulated ranges on return.
566  * \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.
567  * \param threshold [IN] The minimum occupancy threshold to consider a cell to be occupied (Default: 0.5f)
568  * \param rangeNoiseStd [IN] The standard deviation of measurement noise. If not desired, set to 0.
569  * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians).
570  *
571  * \sa laserScanSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
572  */
573  void sonarSimulator(
574  mrpt::obs::CObservationRange &inout_observation,
575  const mrpt::poses::CPose2D &robotPose,
576  float threshold = 0.5f,
577  float rangeNoiseStd = 0.f,
578  float angleNoiseStd = mrpt::utils::DEG2RAD(0.f) ) const;
579 
580  /** 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 */
581  void simulateScanRay(
582  const double x,const double y,const double angle_direction,
583  float &out_range,bool &out_valid,
584  const double max_range_meters,
585  const float threshold_free=0.4f,
586  const double noiseStd=.0, const double angleNoiseStd=.0 ) const;
587 
588  /** Methods for TLaserSimulUncertaintyParams in laserScanSimulatorWithUncertainty() */
590  sumUnscented = 0, //!< Performs an unscented transform
591  sumMonteCarlo //!< Montecarlo-based estimation
592  };
593 
594  /** Input params for laserScanSimulatorWithUncertainty() */
596  {
597  TLaserSimulUncertaintyMethod method; //!< (Default: sumMonteCarlo) Select the method to do the uncertainty propagation
598  /** @name Parameters for each uncertainty method
599  @{ */
600  double UT_alpha, UT_kappa, UT_beta; //!< [sumUnscented] UT parameters. Defaults: alpha=0.99, kappa=0, betta=2.0
601  size_t MC_samples; //!< [sumMonteCarlo] MonteCarlo parameter: number of samples (Default: 10)
602  /** @} */
603 
604  /** @name Generic parameters for all methods
605  @{ */
606  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
607  float aperture; //!< (Default: M_PI) The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
608  bool rightToLeft; //!< (Default: true) The scanning direction: true=counterclockwise; false=clockwise
609  float maxRange; //!< (Default: 80) The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
610  mrpt::poses::CPose3D sensorPose; //!< (Default: at origin) The 6D pose of the sensor on the robot at the moment of starting the scan.
611  size_t nRays;
612  float rangeNoiseStd; //!< (Default: 0) The standard deviation of measurement noise. If not desired, set to 0
613  float angleNoiseStd; //!< (Default: 0) The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians)
614  unsigned int decimation; //!< (Default: 1) The rays that will be simulated are at indexes: 0, D, 2D, 3D,...
615  float threshold; //!< (Default: 0.6f) The minimum occupancy threshold to consider a cell to be occupied
616  /** @} */
617 
619  };
620 
621  /** Output params for laserScanSimulatorWithUncertainty() */
623  {
625 
627  };
628 
629 
630  /** Like laserScanSimulatorWithUncertainty() (see it for a discussion of most parameters) but taking into account
631  * the robot pose uncertainty and generating a vector of predicted variances for each ray.
632  * Range uncertainty includes both, sensor noise and large non-linear effects caused by borders and discontinuities in the environment
633  * as seen from different robot poses.
634  *
635  * \param in_params [IN] Input settings. See TLaserSimulUncertaintyParams
636  * \param in_params [OUT] Output range + uncertainty.
637  *
638  * \sa laserScanSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
639  */
640  void laserScanSimulatorWithUncertainty(const TLaserSimulUncertaintyParams &in_params, TLaserSimulUncertaintyResult &out_results) const;
641 
642  /** @} */
643 
644  /** Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
645  * \param pm The points map
646  * \param relativePose The relative pose of the points map in this map's coordinates, or NULL for (0,0,0).
647  * See "likelihoodOptions" for configuration parameters.
648  */
649  double computeLikelihoodField_Thrun( const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose = NULL);
650 
651  /** Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
652  * \param pm The points map
653  * \param relativePose The relative pose of the points map in this map's coordinates, or NULL for (0,0,0).
654  * See "likelihoodOptions" for configuration parameters.
655  */
656  double computeLikelihoodField_II( const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose = NULL);
657 
658  /** Saves the gridmap as a graphical file (BMP,PNG,...).
659  * The format will be derived from the file extension (see CImage::saveToFile )
660  * \return False on any error.
661  */
662  bool saveAsBitmapFile(const std::string &file) const;
663 
664  /** Saves a composite image with two gridmaps and lines representing a set of correspondences between them.
665  * \sa saveAsEMFTwoMapsWithCorrespondences
666  * \return False on any error.
667  */
668  static bool saveAsBitmapTwoMapsWithCorrespondences(
669  const std::string &fileName,
670  const COccupancyGridMap2D *m1,
671  const COccupancyGridMap2D *m2,
672  const mrpt::utils::TMatchingPairList &corrs);
673 
674  /** Saves a composite image with two gridmaps and numbers for the correspondences between them.
675  * \sa saveAsBitmapTwoMapsWithCorrespondences
676  * \return False on any error.
677  */
678  static bool saveAsEMFTwoMapsWithCorrespondences(
679  const std::string &fileName,
680  const COccupancyGridMap2D *m1,
681  const COccupancyGridMap2D *m2,
682  const mrpt::utils::TMatchingPairList &corrs);
683 
684  /** Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel is 1 cell, and with an overlay of landmarks.
685  * \note The template parameter CLANDMARKSMAP is assumed to be mrpt::maps::CLandmarksMap normally.
686  * \return False on any error.
687  */
688  template <class CLANDMARKSMAP>
690  const std::string &file,
691  const CLANDMARKSMAP *landmarks,
692  bool addTextLabels = false,
693  const mrpt::utils::TColor &marks_color = mrpt::utils::TColor(0,0,255) ) const
694  {
695  MRPT_START
696  using namespace mrpt::utils;
697  CImage img(1,1,3);
698  getAsImageFiltered( img, false, true ); // in RGB
699  const bool topleft = img.isOriginTopLeft();
700  for (unsigned int i=0;i<landmarks->landmarks.size();i++)
701  {
702  const typename CLANDMARKSMAP::landmark_type *lm = landmarks->landmarks.get( i );
703  int px = x2idx( lm->pose_mean.x );
704  int py = topleft ? size_y-1- y2idx( lm->pose_mean.y ) : y2idx( lm->pose_mean.y );
705  img.rectangle( px - 7, (py + 7), px +7, (py -7), marks_color );
706  img.rectangle( px - 6, (py + 6), px +6, (py -6), marks_color );
707  if (addTextLabels)
708  img.textOut(px,py-8,format("%u",i), TColor::black);
709  }
710  return img.saveToFile(file.c_str() );
711  MRPT_END
712  }
713 
714 
715  /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true)
716  * 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.
717  * \sa getAsImageFiltered
718  */
719  void getAsImage( utils::CImage &img, bool verticalFlip = false, bool forceRGB=false, bool tricolor = false) const;
720 
721  /** 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
722  * 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.
723  * \sa getAsImage
724  */
725  void getAsImageFiltered( utils::CImage &img, bool verticalFlip = false, bool forceRGB=false) const;
726 
727  /** 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)
728  */
729  void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE;
730 
731  /** Get a point cloud with all (border) occupied cells as points */
732  void getAsPointCloud( mrpt::maps::CSimplePointsMap &pm, const float occup_threshold = 0.5f ) const;
733 
734  /** Returns true upon map construction or after calling clear(), the return
735  * changes to false upon successful insertObservation() or any other method to load data in the map.
736  */
737  bool isEmpty() const MRPT_OVERRIDE;
738 
739  /** Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
740  * \param file The file to be loaded.
741  * \param resolution The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed.
742  * \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)
743  * \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)
744  * \return False on any error.
745  * \sa loadFromBitmap
746  */
747  bool loadFromBitmapFile(const std::string &file, float resolution, float xCentralPixel = -1, float yCentralPixel =-1 );
748 
749  /** Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
750  * See loadFromBitmapFile() for the meaning of parameters */
751  bool loadFromBitmap(const mrpt::utils::CImage &img, float resolution, float xCentralPixel = -1, float yCentralPixel =-1 );
752 
753  /** See the base class for more details: In this class it is implemented as correspondences of the passed points map to occupied cells.
754  * NOTICE: That the "z" dimension is ignored in the points. Clip the points as appropiated if needed before calling this method.
755  *
756  * \sa computeMatching3DWith
757  */
758  virtual void determineMatching2D(
759  const mrpt::maps::CMetricMap * otherMap,
760  const mrpt::poses::CPose2D & otherMapPose,
761  mrpt::utils::TMatchingPairList & correspondences,
762  const TMatchingParams & params,
763  TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
764 
765 
766  /** See docs in base class: in this class this always returns 0 */
767  float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
768 
769  /** 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). */
770  void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE;
771 
772  /** The structure used to store the set of Voronoi diagram
773  * critical points.
774  * \sa findCriticalPoints
775  */
777  {
778  TCriticalPointsList() : x(),y(),clearance(),x_basis1(),y_basis1(),x_basis2(),y_basis2()
779  {}
780 
781  std::vector<int> x,y; //!< The coordinates of critical point.
782  std::vector<int> clearance; //!< The clearance of critical points, in 1/100 of cells.
783  std::vector<int> x_basis1,y_basis1, x_basis2,y_basis2; //!< Their two first basis points coordinates.
784  } CriticalPointsList;
785 
786 
787  private:
788  // See docs in base class
789  double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) MRPT_OVERRIDE;
790  // See docs in base class
791  bool internal_canComputeObservationLikelihood( const mrpt::obs::CObservation *obs ) const MRPT_OVERRIDE;
792 
793  /** Returns a byte with the occupancy of the 8 sorrounding cells.
794  * \param cx The cell index
795  * \param cy The cell index
796  * \sa direction2idx
797  */
798  inline unsigned char GetNeighborhood( int cx, int cy ) const;
799 
800  /** Used to store the 8 possible movements from a cell to the
801  * sorrounding ones.Filled in the constructor.
802  * \sa direction2idx
803  */
804  int direccion_vecino_x[8],direccion_vecino_y[8];
805 
806  /** Returns the index [0,7] of the given movement, or
807  * -1 if invalid one.
808  * \sa direccion_vecino_x,direccion_vecino_y,GetNeighborhood
809  */
810  int direction2idx(int dx, int dy);
811 
812 
814  float min_x,max_x,min_y,max_y,resolution; //!< See COccupancyGridMap2D::COccupancyGridMap2D
815  mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts; //!< Observations insertion options
816  mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts; //!< Probabilistic observation likelihood options
818 
819  };
820  DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE( COccupancyGridMap2D, CMetricMap, MAPS_IMPEXP )
821 
822 
823  bool MAPS_IMPEXP operator <(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2);
824 
825  } // End of namespace
826 } // End of namespace
827 
828 #endif
virtual void textOut(int x0, int y0, const std::string &str, const mrpt::utils::TColor color)
Renders 2D text using bitmap fonts.
Definition: CCanvas.cpp:397
mrpt::poses::CPosePDFGaussian robotPose
The robot pose Gaussian, in map coordinates. Recall that sensor pose relative to this robot pose must...
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:223
const GLdouble * v
Definition: glew.h:1296
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.
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
int x2idx(float x) const
Transform a coordinate value into a cell index.
Parameters for CMetricMap::compute3DMatchingRatio()
#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)
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
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
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...
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
bool isOriginTopLeft() const
Returns true if the coordinates origin is top-left, or false if it is bottom-left.
Definition: CImage.cpp:927
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.
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.
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.
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
void rectangle(int x0, int y0, int x1, int y1, const mrpt::utils::TColor color, unsigned int width=1)
Draws a rectangle (an empty rectangle, without filling)
Definition: CCanvas.cpp:176
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
float getResolution() const
Returns the resolution of the grid map.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
bool rightToLeft
(Default: true) The scanning direction: true=counterclockwise; false=clockwise
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...
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:220
std::vector< cellType > map
Store of cell occupancy values. Order: row by row, from left to right.
bool isStaticCell(int cx, int cy, float threshold=0.7f) const
GLdouble l
Definition: glew.h:5092
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)
GLsizei const GLfloat * value
Definition: glew.h:1756
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)...
GLint GLvoid * img
Definition: glew.h:1290
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
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...
const cellType * getRow(int cy) const
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally...
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
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 saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV)...
Definition: CImage.cpp:299
float rayTracing_stdHit
[rayTracing] The laser range sigma.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
TLaserSimulUncertaintyMethod
Methods for TLaserSimulUncertaintyParams in laserScanSimulatorWithUncertainty()
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)
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
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
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...
float getXMin() const
Returns the "x" coordinate of left side of grid map.
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,...
GLfloat GLfloat p
Definition: glew.h:10113
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2) ...
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
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
uint16_t getVoroniClearance(int cx, int cy) const
Reads a the clearance of a cell (in centimeters), after building the Voronoi diagram with buildVorono...
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.
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
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
unsigned char getBasisCell(int x, int y) const
Reads a cell in the "basis" maps.Used for Voronoi calculation.
float getXMax() const
Returns the "x" coordinate of right side of grid map.
#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)
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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's observation.
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
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.
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...
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...
int y2idx(float y, float y_min) const
TLaserSimulUncertaintyMethod method
(Default: sumMonteCarlo) Select the method to do the uncertainty propagation
bool m_is_empty
True upon construction; used by isEmpty()
const mrpt::utils::CDynamicGrid< uint8_t > & getBasisMap() const
Return the auxiliary "basis" map built while building the Voronoi diagram.
bool useMapAltitude
The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true...
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 ...
float mapAltitude
The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map! ...
GLfloat * params
Definition: glew.h:1436
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...
std::vector< TPairLikelihoodIndex > OWA_pairList
[OWA method] This will contain the ascending-ordered list of pairs:(likelihood values, 2D point in map coordinates).
int x2idx(float x, float x_min) const
Transform a coordinate value into a cell index, using a diferent "x_min" value.
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
float maxRange
(Default: 80) The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0...
This class stores any customizable set of metric maps.
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...
std::vector< int > clearance
The clearance of critical points, in 1/100 of cells.
int16_t cellType
The type of the map cells:
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() ...
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
void setPos(float x, float y, float value)
Change the contents [0,1] of a cell, given its coordinates.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
float getCell_nocheck(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
static float l2p(const cellType l)
Scales an integer representation of the log-odd into a real valued probability in [0...
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
bool enabled
If set to false (default), this struct is not used. Set to true only when measuring the info of an ob...
float idx2y(const size_t cy) const
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018