Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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>
24#include <mrpt/obs/obs_frwds.h>
25
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
33namespace mrpt
34{
35namespace maps
36{
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;
80 typedef uint16_t cellTypeUnsigned;
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.) */
140 /** One of the methods that can be selected for implementing "computeObservationLikelihood". TODO: This method is described in.... */
142 /** One of the methods that can be selected for implementing "computeObservationLikelihood" */
144 /** One of the methods that can be selected for implementing "computeObservationLikelihood" */
146 /** One of the methods that can be selected for implementing "computeObservationLikelihood" */
148 /** One of the methods that can be selected for implementing "computeObservationLikelihood".*/
150 /** One of the methods that can be selected for implementing "computeObservationLikelihood". */
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 */
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::utils::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 */
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 */
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 */
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 */
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 */
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 */
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 {
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() );
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) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map.
741 * \param yCentralPixel The "y" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map.
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 * \param img The image. Only a grayscale image will be used, so RGB components will be mixed if a color image is passed.
749 * \param resolution The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed.
750 * \param xCentralPixel The "x" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map.
751 * \param yCentralPixel The "y" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map.
752 * \return False on any error.
753 * \sa loadFromBitmapFile
754 */
755 bool loadFromBitmap(const mrpt::utils::CImage &img, float resolution, float xCentralPixel = -1, float yCentralPixel =-1 );
756
757 /** See the base class for more details: In this class it is implemented as correspondences of the passed points map to occupied cells.
758 * NOTICE: That the "z" dimension is ignored in the points. Clip the points as appropiated if needed before calling this method.
759 *
760 * \sa computeMatching3DWith
761 */
762 virtual void determineMatching2D(
763 const mrpt::maps::CMetricMap * otherMap,
764 const mrpt::poses::CPose2D & otherMapPose,
765 mrpt::utils::TMatchingPairList & correspondences,
766 const TMatchingParams & params,
767 TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
768
769
770 /** See docs in base class: in this class this always returns 0 */
771 float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
772
773 /** 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). */
774 void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE;
775
776 /** The structure used to store the set of Voronoi diagram
777 * critical points.
778 * \sa findCriticalPoints
779 */
781 {
782 TCriticalPointsList() : x(),y(),clearance(),x_basis1(),y_basis1(),x_basis2(),y_basis2()
783 {}
784
785 std::vector<int> x,y; //!< The coordinates of critical point.
786 std::vector<int> clearance; //!< The clearance of critical points, in 1/100 of cells.
787 std::vector<int> x_basis1,y_basis1, x_basis2,y_basis2; //!< Their two first basis points coordinates.
788 } CriticalPointsList;
789
790
791 private:
792 // See docs in base class
794 // See docs in base class
796
797 /** Returns a byte with the occupancy of the 8 sorrounding cells.
798 * \param cx The cell index
799 * \param cy The cell index
800 * \sa direction2idx
801 */
802 inline unsigned char GetNeighborhood( int cx, int cy ) const;
803
804 /** Used to store the 8 possible movements from a cell to the
805 * sorrounding ones.Filled in the constructor.
806 * \sa direction2idx
807 */
808 int direccion_vecino_x[8],direccion_vecino_y[8];
809
810 /** Returns the index [0,7] of the given movement, or
811 * -1 if invalid one.
812 * \sa direccion_vecino_x,direccion_vecino_y,GetNeighborhood
813 */
814 int direction2idx(int dx, int dy);
815
816
818 float min_x,max_x,min_y,max_y,resolution; //!< See COccupancyGridMap2D::COccupancyGridMap2D
819 mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts; //!< Observations insertion options
820 mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts; //!< Probabilistic observation likelihood options
822
823 };
825
826
827 bool MAPS_IMPEXP operator <(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2);
828
829 } // End of namespace
830} // End of namespace
831
832#endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#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...
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ... MAP_DEFINITION_END() block inside the declaration of each metric map...
Declares a virtual base class for all metric maps storage classes.
This class stores any customizable set of metric maps.
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
With this struct options are provided to the observation insertion process.
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0....
float mapAltitude
The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map!
bool useMapAltitude
The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true.
float CFD_features_gaussian_size
Gaussian sigma of the filter used in getAsImageFiltered (for features detection) (Default=1) (0:Disab...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini" file.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal,...
TInsertionOptions()
Initilization of default parameters.
float maxDistanceInsertion
The largest distance at which cells will be updated (Default 15 meters)
float CFD_features_median_size
Size of the Median filter used in getAsImageFiltered (for features detection) (Default=3) (0:Disabled...
bool considerInvalidRangesAsFreeSpace
If set to true (default), invalid range values (no echo rays) as consider as free space until "maxOcc...
uint16_t decimation
Specify the decimation of the range scan (default=1 : take all the range values!)
bool wideningBeamsWithDistance
Enabled: Rays widen with distance to approximate the real behavior of lasers, disabled: insert rays a...
With this struct options are provided to the observation likelihood computation process.
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
bool enableLikelihoodCache
Enables the usage of a cache of likelihood values (for LF methods), if set to true (default=false).
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0....
float consensus_pow
[Consensus] The power factor for the likelihood (default=5)
float LF_maxCorrsDistance
[LikelihoodField] The max. distance for searching correspondences around each sensed point
uint32_t MI_skip_rays
[MI] The scan rays decimation: at every N rays, one will be used to compute the MI
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
float LF_maxRange
[LikelihoodField] The max. range of the sensor (Default= 81 m)
TLikelihoodOptions()
Initilization of default parameters.
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2)
float MI_ratio_max_distance
[MI] The ratio for the max. distance used in the MI computation and in the insertion of scans,...
float rayTracing_stdHit
[rayTracing] The laser range sigma.
bool LF_alternateAverageMethod
[LikelihoodField] Set this to "true" ot use an alternative method, where the likelihood of the whole ...
std::vector< float > OWA_weights
[OWA] The sequence of weights to be multiplied to of the ordered list of likelihood values (first one...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini" file.
float MI_exponent
[MI] The exponent in the MI likelihood computation. Default value = 5
int32_t consensus_takeEachRange
[Consensus] The down-sample ratio of ranges (default=1, consider all the ranges)
bool rayTracing_useDistanceFilter
[rayTracing] If true (default), the rayTracing method will ignore measured ranges shorter than the si...
Some members of this struct will contain intermediate or output data after calling "computeObservatio...
std::vector< double > OWA_individualLikValues
[OWA method] This will contain the ascending-ordered list of likelihood values for individual range m...
std::vector< TPairLikelihoodIndex > OWA_pairList
[OWA method] This will contain the ascending-ordered list of pairs:(likelihood values,...
A class for storing an occupancy grid map.
virtual ~COccupancyGridMap2D()
Destructor.
void setRawCell(unsigned int cellIndex, cellType b)
Changes a cell by its absolute index (Do not use it normally)
std::vector< double > precomputedLikelihood
Auxiliary variables to speed up the computation of observation likelihood values for LF method among ...
static std::vector< float > entropyTable
Internally used to speed-up entropy calculation.
TLaserSimulUncertaintyMethod
Methods for TLaserSimulUncertaintyParams in laserScanSimulatorWithUncertainty()
void getAsPointCloud(mrpt::maps::CSimplePointsMap &pm, const float occup_threshold=0.5f) const
Get a point cloud with all (border) occupied cells as points.
float resolution
Cell size, i.e. resolution of the grid map.
double computeObservationLikelihood_MI(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
void setCell(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
double computeLikelihoodField_II(const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose=NULL)
Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
void buildVoronoiDiagram(float threshold, float robot_size, int x1=0, int x2=0, int y1=0, int y2=0)
Build the Voronoi diagram of the grid map.
double computeObservationLikelihood_Consensus(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood" (This method ...
void subSample(int downRatio)
Performs a downsampling of the gridmap, by a given factor: resolution/=ratio.
mrpt::utils::CDynamicGrid< uint16_t > m_voronoi_diagram
Used to store the Voronoi diagram.
void setBasisCell(int x, int y, uint8_t value)
Change a cell in the "basis" maps.Used for Voronoi calculation.
bool isStaticCell(int cx, int cy, float threshold=0.7f) const
uint16_t getVoroniClearance(int cx, int cy) const
Reads a the clearance of a cell (in centimeters), after building the Voronoi diagram with buildVorono...
unsigned char GetNeighborhood(int cx, int cy) const
Returns a byte with the occupancy of the 8 sorrounding cells.
void findCriticalPoints(float filter_distance)
Builds a list with the critical points from Voronoi diagram, which must must be built before calling ...
int direction2idx(int dx, int dy)
Returns the index [0,7] of the given movement, or -1 if invalid one.
float getResolution() const
Returns the resolution of the grid map.
const mrpt::utils::CDynamicGrid< uint8_t > & getBasisMap() const
Return the auxiliary "basis" map built while building the Voronoi diagram.
void setCell_nocheck(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
static CLogOddsGridMapLUT< cellType > m_logodd_lut
Lookup tables for log-odds.
static float l2p(const cellType l)
Scales an integer representation of the log-odd into a real valued probability in [0,...
float computePathCost(float x1, float y1, float x2, float y2) const
Compute the 'cost' of traversing a segment of the map according to the occupancy of traversed cells.
void freeMap()
Frees the dynamic memory buffers of map.
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)...
static double H(double p)
Entropy computation internal function:
double computeObservationLikelihood_likelihoodField_Thrun(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
int x2idx(float x, float x_min) const
Transform a coordinate value into a cell index, using a diferent "x_min" value.
const mrpt::utils::CDynamicGrid< uint16_t > & getVoronoiDiagram() const
Return the Voronoi diagram; each cell contains the distance to its closer obstacle,...
virtual void internal_clear() MRPT_OVERRIDE
Clear the map: It set all cells to their default occupancy value (0.5), without changing the resoluti...
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...
float computeClearance(float x, float y, float maxSearchDistance) const
An alternative method for computing the clearance of a given location (in meters).
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.
float voroni_free_threshold
The free-cells threshold used to compute the Voronoi diagram.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
TLikelihoodMethod
The type for selecting a likelihood computation method.
void laserScanSimulator(mrpt::obs::CObservation2DRangeScan &inout_Scan, const mrpt::poses::CPose2D &robotPose, float threshold=0.6f, size_t N=361, float noiseStd=0, unsigned int decimation=1, float angleNoiseStd=mrpt::utils::DEG2RAD(0)) const
Simulates a laser range scan into the current grid map.
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
void sonarSimulator(mrpt::obs::CObservationRange &inout_observation, const mrpt::poses::CPose2D &robotPose, float threshold=0.5f, float rangeNoiseStd=0.f, float angleNoiseStd=mrpt::utils::DEG2RAD(0.f)) const
Simulates the observations of a sonar rig into the current grid map.
float idx2y(const size_t cy) const
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
void computeEntropy(TEntropyInfo &info) const
Computes the entropy and related values of this grid map.
std::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
COccupancyGridMap2D(float min_x=-20.0f, float max_x=20.0f, float min_y=-20.0f, float max_y=20.0f, float resolution=0.05f)
Constructor.
bool saveAsBitmapFile(const std::string &file) const
Saves the gridmap as a graphical file (BMP,PNG,...).
static bool saveAsEMFTwoMapsWithCorrespondences(const std::string &fileName, const COccupancyGridMap2D *m1, const COccupancyGridMap2D *m2, const mrpt::utils::TMatchingPairList &corrs)
Saves a composite image with two gridmaps and numbers for the correspondences between them.
cellType * getRow(int cy)
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
double computeObservationLikelihood_CellsDifference(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood"
int x2idx(float x) const
Transform a coordinate value into a cell index.
bool m_is_empty
True upon construction; used by isEmpty()
static bool saveAsBitmapTwoMapsWithCorrespondences(const std::string &fileName, const COccupancyGridMap2D *m1, const COccupancyGridMap2D *m2, const mrpt::utils::TMatchingPairList &corrs)
Saves a composite image with two gridmaps and lines representing a set of correspondences between the...
void updateCell(int x, int y, float v)
Performs the Bayesian fusion of a new observation of a cell.
int y2idx(float y, float y_min) const
float getXMax() const
Returns the "x" coordinate of right side of grid map.
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
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,...
void getAsImageFiltered(utils::CImage &img, bool verticalFlip=false, bool forceRGB=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
const cellType * getRow(int cy) const
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
std::vector< cellType > map
Store of cell occupancy values. Order: row by row, from left to right.
double computeObservationLikelihood_rayTracing(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
unsigned char getBasisCell(int x, int y) const
Reads a cell in the "basis" maps.Used for Voronoi calculation.
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
void setVoroniClearance(int cx, int cy, uint16_t dist)
Used to set the clearance of a cell, while building the Voronoi diagram.
double computeObservationLikelihood_ConsensusOWA(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
void resizeGrid(float new_x_min, float new_x_max, float new_y_min, float new_y_max, float new_cells_default_value=0.5f, bool additionalMargin=true) MRPT_NO_THROWS
Change the size of gridmap, maintaining previous contents.
bool isEmpty() const MRPT_OVERRIDE
Returns true upon map construction or after calling clear(), the return changes to false upon success...
void fill(float default_value=0.5f)
Fills all the cells with a default value.
double computeObservationLikelihood_likelihoodField_II(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
One of the methods that can be selected for implementing "computeObservationLikelihood".
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
float getXMin() const
Returns the "x" coordinate of left side of grid map.
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D plane with its texture being the occupancy grid and transparency proportional to "uncert...
static cellType p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
void copyMapContentFrom(const COccupancyGridMap2D &otherMap)
copy the gridmap contents, but not all the options, from another map instance
void setPos(float x, float y, float value)
Change the contents [0,1] of a cell, given its coordinates.
float getCell_nocheck(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
int16_t cellType
The type of the map cells:
void setSize(float x_min, float x_max, float y_min, float y_max, float resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
virtual void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation *) MRPT_OVERRIDE
See base class.
void simulateScanRay(const double x, const double y, const double angle_direction, float &out_range, bool &out_valid, const double max_range_meters, const float threshold_free=0.4f, const double noiseStd=.0, const double angleNoiseStd=.0) const
Simulate just one "ray" in the grid map.
int computeClearance(int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint=false) const
Compute the clearance of a given cell, and returns its two first basis (closest obstacle) points....
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation *obs) MRPT_OVERRIDE
Internal method called by canComputeObservationLikelihood()
void getAsImage(utils::CImage &img, bool verticalFlip=false, bool forceRGB=false, bool tricolor=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
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 computeLikelihoodField_Thrun(const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose=NULL)
Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
void laserScanSimulatorWithUncertainty(const TLaserSimulUncertaintyParams &in_params, TLaserSimulUncertaintyResult &out_results) const
Like laserScanSimulatorWithUncertainty() (see it for a discussion of most parameters) but taking into...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A 2D range scan plus an uncertainty model for each range.
Declares a class that represents any robot's observation.
Declares a class derived from "CObservation" that encapsules a single range measurement,...
A class used to store a 2D pose.
Definition: CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
virtual void textOut(int x0, int y0, const std::string &str, const mrpt::utils::TColor color)
Renders 2D text using bitmap fonts.
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)
This class allows loading and storing values and vectors of different types from a configuration text...
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:41
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:220
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
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:223
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:102
bool isOriginTopLeft() const
Returns true if the coordinates origin is top-left, or false if it is bottom-left
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).
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
A list of TMatchingPair.
Definition: TMatchingPair.h:67
#define MRPT_START
Definition: mrpt_macros.h:349
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
#define MRPT_END
Definition: mrpt_macros.h:353
#define ASSERT_BELOWEQ_(__A, __B)
Definition: mrpt_macros.h:268
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: mrpt_macros.h:269
#define MRPT_NO_THROWS
Used after member declarations.
Definition: mrpt_macros.h:391
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: zip.h:16
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:69
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:113
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
STL namespace.
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
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)...
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0,...
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...
The structure used to store the set of Voronoi diagram critical points.
std::vector< int > clearance
The clearance of critical points, in 1/100 of cells.
Used for returning entropy related information.
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
double H
The target variable for absolute entropy, computed as:
double effectiveMappedArea
The target variable for the area of cells with information, i.e. p(x)!=0.5.
double mean_H
The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells)
unsigned long effectiveMappedCells
The mapped area in cells.
double mean_I
The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (ce...
Input params for laserScanSimulatorWithUncertainty()
unsigned int decimation
(Default: 1) The rays that will be simulated are at indexes: 0, D, 2D, 3D,...
mrpt::poses::CPosePDFGaussian robotPose
The robot pose Gaussian, in map coordinates. Recall that sensor pose relative to this robot pose must...
float aperture
(Default: M_PI) The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180...
float threshold
(Default: 0.6f) The minimum occupancy threshold to consider a cell to be occupied
float rangeNoiseStd
(Default: 0) The standard deviation of measurement noise. If not desired, set to 0
float angleNoiseStd
(Default: 0) The sigma of an optional Gaussian noise added to the angles at which ranges are measured...
bool rightToLeft
(Default: true) The scanning direction: true=counterclockwise; false=clockwise
TLaserSimulUncertaintyMethod method
(Default: sumMonteCarlo) Select the method to do the uncertainty propagation
mrpt::poses::CPose3D sensorPose
(Default: at origin) The 6D pose of the sensor on the robot at the moment of starting the scan.
size_t MC_samples
[sumMonteCarlo] MonteCarlo parameter: number of samples (Default: 10)
float maxRange
(Default: 80) The maximum range allowed by the device, in meters (e.g. 80m, 50m,.....
Output params for laserScanSimulatorWithUncertainty()
mrpt::obs::CObservation2DRangeScanWithUncertainty scanWithUncert
The scan + its uncertainty.
An internal structure for storing data related to counting the new information apported by some obser...
bool enabled
If set to false (default), this struct is not used. Set to true only when measuring the info of an ob...
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
TUpdateCellsInfoChangeOnly(bool enabled=false, double I_change=0, int cellsUpdated=0)
int laserRaysSkip
In this mode, some laser rays can be skips to speep-up.
double I_change
The cummulative change in Information: This is updated only from the "updateCell" method.
Additional results from the determination of matchings between point clouds, etc.,...
Parameters for the determination of matchings between point clouds, etc.
Parameters for CMetricMap::compute3DMatchingRatio()
A RGB color - 8bit.
Definition: TColor.h:26



Page generated by Doxygen 1.9.2 for MRPT 1.4.0 SVN: at Mon Sep 20 00:36:32 UTC 2021