Go to the documentation of this file.
10 #ifndef COccupancyGridMap2D_H
11 #define COccupancyGridMap2D_H
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.
64 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
75 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
98 std::vector<cellType>
map;
120 static double H(
double p);
125 map[x+y*size_x]=p2l(value);
130 return l2p(map[x+y*size_x]);
134 if (cellIndex<size_x*size_y)
164 virtual
bool internal_insertObservation( const
mrpt::obs::CObservation *obs, const
mrpt::poses::CPose3D *robotPose = NULL )
MRPT_OVERRIDE;
168 const std::vector<
cellType> & getRawMap()
const {
return this->map; }
170 void updateCell(
int x,
int y,
float v);
175 TUpdateCellsInfoChangeOnly(
bool enabled =
false,
double I_change = 0,
int cellsUpdated=0) : enabled(enabled), I_change(I_change), cellsUpdated(cellsUpdated), laserRaysSkip(1)
182 } updateInfoChangeOnly;
184 void fill(
float default_value = 0.5f );
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 );
200 void setSize(
float x_min,
float x_max,
float y_min,
float y_max,
float resolution,
float default_value = 0.5f);
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;
217 inline unsigned int getSizeX()
const {
return size_x; }
220 inline unsigned int getSizeY()
const {
return size_y; }
223 inline float getXMin()
const {
return x_min; }
226 inline float getXMax()
const {
return x_max; }
229 inline float getYMin()
const {
return y_min; }
232 inline float getYMax()
const {
return y_max; }
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 ); }
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 ); }
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; }
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 ); }
254 return m_logodd_lut.
l2p(l);
258 return m_logodd_lut.
l2p_255(l);
262 return m_logodd_lut.
p2l(p);
269 if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
271 else map[x+y*size_x]=p2l(value);
278 if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
280 else return l2p(map[x+y*size_x]);
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]; }
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]; }
290 inline void setPos(
float x,
float y,
float value) { setCell(x2idx(x),y2idx(y),value); }
293 inline float getPos(
float x,
float y)
const {
return getCell(x2idx(x),y2idx(y)); }
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); }
330 TEntropyInfo() : H(0),I(0),mean_H(0),mean_I(0),effectiveMappedArea(0),effectiveMappedCells(0)
381 lmMeanInformation = 0,
444 void subSample(
int downRatio );
463 void buildVoronoiDiagram(
float threshold,
float robot_size,
int x1=0,
int x2=0,
int y1=0,
int y2=0);
474 const uint16_t *cell=m_voronoi_diagram.
cellByIndex(cx,cy);
482 uint16_t *cell=m_voronoi_diagram.
cellByIndex(cx,cy);
505 void findCriticalPoints(
float filter_distance );
521 int computeClearance(
int cx,
int cy,
int *basis_x,
int *basis_y,
int *nBasis,
bool GetContourPoint =
false )
const;
526 float computeClearance(
float x,
float y,
float maxSearchDistance )
const;
531 float computePathCost(
float x1,
float y1,
float x2,
float y2 )
const;
551 void laserScanSimulator(
554 float threshold = 0.6f,
557 unsigned int decimation = 1,
574 float threshold = 0.5f,
575 float rangeNoiseStd = 0.f,
579 void simulateScanRay(
580 const double x,
const double y,
const double angle_direction,
581 float &out_range,
bool &out_valid,
582 const double max_range_meters,
583 const float threshold_free=0.4f,
584 const double noiseStd=.0,
const double angleNoiseStd=.0 )
const;
660 bool saveAsBitmapFile(
const std::string &file)
const;
666 static bool saveAsBitmapTwoMapsWithCorrespondences(
667 const std::string &fileName,
676 static bool saveAsEMFTwoMapsWithCorrespondences(
677 const std::string &fileName,
686 template <
class CLANDMARKSMAP>
688 const std::string &file,
689 const CLANDMARKSMAP *landmarks,
690 bool addTextLabels =
false,
696 getAsImageFiltered( img,
false,
true );
698 for (
unsigned int i=0;i<landmarks->landmarks.size();i++)
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 );
717 void getAsImage(
utils::CImage &img,
bool verticalFlip =
false,
bool forceRGB=
false,
bool tricolor =
false)
const;
723 void getAsImageFiltered(
utils::CImage &img,
bool verticalFlip =
false,
bool forceRGB=
false)
const;
727 void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj)
const MRPT_OVERRIDE;
745 bool loadFromBitmapFile(const std::
string &file,
float resolution,
float xCentralPixel = -1,
float yCentralPixel =-1 );
755 bool loadFromBitmap(const
mrpt::utils::
CImage &img,
float resolution,
float xCentralPixel = -1,
float yCentralPixel =-1 );
762 virtual
void determineMatching2D(
764 const
mrpt::poses::CPose2D & otherMapPose,
774 void saveMetricMapRepresentationToFile(const std::
string &filNamePrefix) const
MRPT_OVERRIDE;
785 std::vector<int> x,
y;
787 std::vector<int> x_basis1,y_basis1, x_basis2,
y_basis2;
788 } CriticalPointsList;
802 inline unsigned char GetNeighborhood(
int cx,
int cy )
const;
808 int direccion_vecino_x[8],direccion_vecino_y[8];
814 int direction2idx(
int dx,
int dy);
818 float min_x,max_x,min_y,max_y,resolution;
int y2idx(double y) const
bool LF_alternateAverageMethod
[LikelihoodField] Set this to "true" ot use an alternative method, where the likelihood of the whole ...
float LF_maxRange
[LikelihoodField] The max. range of the sensor (Default= 81 m)
bool precomputedLikelihoodToBeRecomputed
bool isOriginTopLeft() const
Returns true if the coordinates origin is top-left, or false if it is bottom-left
float maxDistanceInsertion
The largest distance at which cells will be updated (Default 15 meters)
float LF_zRandom
[LikelihoodField]
bool enabled
If set to false (default), this struct is not used. Set to true only when measuring the info of an ob...
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2)
An internal structure for storing data related to counting the new information apported by some obser...
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0....
A 2D range scan plus an uncertainty model for each range.
void setCell_nocheck(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
cellType * getRow(int cy)
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
bool useMapAltitude
The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true.
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.
TLaserSimulUncertaintyMethod method
(Default: sumMonteCarlo) Select the method to do the uncertainty propagation
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0,...
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
double mean_I
The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (ce...
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.
float voroni_free_threshold
The free-cells threshold used to compute the Voronoi diagram.
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...
mrpt::poses::CPose3D sensorPose
(Default: at origin) The 6D pose of the sensor on the robot at the moment of starting the scan.
std::vector< int > y_basis2
Their two first basis points coordinates.
Parameters for CMetricMap::compute3DMatchingRatio()
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
float aperture
(Default: M_PI) The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180...
#define MRPT_NO_THROWS
Used after member declarations.
int x2idx(double x) const
int x2idx(float x, float x_min) const
Transform a coordinate value into a cell index, using a diferent "x_min" value.
void setPos(float x, float y, float value)
Change the contents [0,1] of a cell, given its coordinates.
void setBasisCell(int x, int y, uint8_t value)
Change a cell in the "basis" maps.Used for Voronoi calculation.
float rangeNoiseStd
(Default: 0) The standard deviation of measurement noise. If not desired, set to 0
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal,...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Some members of this struct will contain intermediate or output data after calling "computeObservatio...
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...
static float l2p(const cellType l)
Scales an integer representation of the log-odd into a real valued probability in [0,...
size_t MC_samples
[sumMonteCarlo] MonteCarlo parameter: number of samples (Default: 10)
A class for storing images as grayscale or RGB bitmaps.
float getXMax() const
Returns the "x" coordinate of right side of grid 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)...
bool m_is_empty
True upon construction; used by isEmpty()
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0....
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
float idx2y(const size_t cy) const
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...
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
Output params for laserScanSimulatorWithUncertainty()
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Declares a virtual base class for all metric maps storage classes.
double DEG2RAD(const double x)
Degrees to radians.
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ...
double I_change
The cummulative change in Information: This is updated only from the "updateCell" method.
void setCell(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
static CLogOddsGridMapLUT< cellType > m_logodd_lut
Lookup tables for log-odds.
bool rayTracing_useDistanceFilter
[rayTracing] If true (default), the rayTracing method will ignore measured ranges shorter than the si...
int y2idx(float y, float y_min) const
std::vector< TPairLikelihoodIndex > OWA_pairList
[OWA method] This will contain the ascending-ordered list of pairs:(likelihood values,...
float consensus_pow
[Consensus] The power factor for the likelihood (default=5)
double H
The target variable for absolute entropy, computed as:
std::vector< float > OWA_weights
[OWA] The sequence of weights to be multiplied to of the ordered list of likelihood values (first one...
float mapAltitude
The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map!
uint16_t cellTypeUnsigned
TUpdateCellsInfoChangeOnly(bool enabled=false, double I_change=0, int cellsUpdated=0)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
unsigned int decimation
(Default: 1) The rays that will be simulated are at indexes: 0, D, 2D, 3D,...
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
virtual void textOut(int x0, int y0, const std::string &str, const mrpt::utils::TColor color)
Renders 2D text using bitmap fonts.
A class used to store a 2D pose.
This class stores any customizable set of metric maps.
float LF_maxCorrsDistance
[LikelihoodField] The max. distance for searching correspondences around each sensed point
mrpt::utils::CDynamicGrid< uint16_t > m_voronoi_diagram
Used to store the Voronoi diagram.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
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::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
int16_t cellType
The type of the map cells:
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation
int x2idx(float x) const
Transform a coordinate value into a cell index.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
bool considerInvalidRangesAsFreeSpace
If set to true (default), invalid range values (no echo rays) as consider as free space until "maxOcc...
unsigned long effectiveMappedCells
The mapped area in cells.
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
bool enableLikelihoodCache
Enables the usage of a cache of likelihood values (for LF methods), if set to true (default=false).
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
mrpt::poses::CPosePDFGaussian robotPose
The robot pose Gaussian, in map coordinates. Recall that sensor pose relative to this robot pose must...
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
uint16_t getVoroniClearance(int cx, int cy) const
Reads a the clearance of a cell (in centimeters), after building the Voronoi diagram with buildVorono...
bool wideningBeamsWithDistance
Enabled: Rays widen with distance to approximate the real behavior of lasers, disabled: insert rays a...
double mean_H
The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells)
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).
float MI_ratio_max_distance
[MI] The ratio for the max. distance used in the MI computation and in the insertion of scans,...
int32_t consensus_takeEachRange
[Consensus] The down-sample ratio of ranges (default=1, consider all the ranges)
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::vector< cellType > map
Store of cell occupancy values. Order: row by row, from left to right.
Parameters for the determination of matchings between point clouds, etc.
const cellType * getRow(int cy) const
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
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.
Used for returning entropy related information.
std::vector< int > clearance
The clearance of critical points, in 1/100 of cells.
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,...
Declares a class derived from "CObservation" that encapsules a single range measurement,...
uint16_t decimation
Specify the decimation of the range scan (default=1 : take all the range values!)
float CFD_features_median_size
Size of the Median filter used in getAsImageFiltered (for features detection) (Default=3) (0:Disabled...
float resolution
Cell size, i.e. resolution of the grid map.
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
#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...
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)
double effectiveMappedArea
The target variable for the area of cells with information, i.e. p(x)!=0.5.
TLikelihoodMethod
The type for selecting a likelihood computation method.
bool rightToLeft
(Default: true) The scanning direction: true=counterclockwise; false=clockwise
float getResolution() const
Returns the resolution of the grid map.
const mrpt::utils::CDynamicGrid< uint16_t > & getVoronoiDiagram() const
Return the Voronoi diagram; each cell contains the distance to its closer obstacle,...
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.
float angleNoiseStd
(Default: 0) The sigma of an optional Gaussian noise added to the angles at which ranges are measured...
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 maxRange
(Default: 80) The maximum range allowed by the device, in meters (e.g. 80m, 50m,.....
float getXMin() const
Returns the "x" coordinate of left side of grid map.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
bool isStaticCell(int cx, int cy, float threshold=0.7f) const
T square(const T x)
Inline function for the square of a number.
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
With this struct options are provided to the observation insertion process.
void setRawCell(unsigned int cellIndex, cellType b)
Changes a cell by its absolute index (Do not use it normally)
float getCell_nocheck(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
#define ASSERT_ABOVEEQ_(__A, __B)
TLaserSimulUncertaintyMethod
Methods for TLaserSimulUncertaintyParams in laserScanSimulatorWithUncertainty()
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
Declares a class that represents any robot's observation.
A class for storing an occupancy grid map.
float rayTracing_stdHit
[rayTracing] The laser range sigma.
With this struct options are provided to the observation likelihood computation process.
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
uint32_t MI_skip_rays
[MI] The scan rays decimation: at every N rays, one will be used to compute the MI
EIGEN_STRONG_INLINE void fill(const Scalar v)
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
const mrpt::utils::CDynamicGrid< uint8_t > & getBasisMap() const
Return the auxiliary "basis" map built while building the Voronoi diagram.
std::vector< double > OWA_individualLikValues
[OWA method] This will contain the ascending-ordered list of likelihood values for individual range m...
int laserRaysSkip
In this mode, some laser rays can be skips to speep-up.
#define ASSERT_BELOWEQ_(__A, __B)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Input params for laserScanSimulatorWithUncertainty()
std::vector< int > y
The coordinates of critical point.
std::vector< double > precomputedLikelihood
Auxiliary variables to speed up the computation of observation likelihood values for LF method among ...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
float threshold
(Default: 0.6f) The minimum occupancy threshold to consider a cell to be occupied
mrpt::obs::CObservation2DRangeScanWithUncertainty scanWithUncert
The scan + its uncertainty.
static std::vector< float > entropyTable
Internally used to speed-up entropy calculation.
unsigned char getBasisCell(int x, int y) const
Reads a cell in the "basis" maps.Used for Voronoi calculation.
float MI_exponent
[MI] The exponent in the MI likelihood computation. Default value = 5
Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 23:11:08 UTC 2019 | | |