10 #ifndef COccupancyGridMap2D_H 11 #define COccupancyGridMap2D_H 26 #include <mrpt/config.h> 27 #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)) 28 #error One of OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS or OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS must be defined. 62 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS 73 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS 97 std::vector<cellType>
map;
139 static double H(
double p);
145 map[x+y*size_x]=p2l(value);
152 return l2p(map[x+y*size_x]);
159 if (cellIndex<size_x*size_y)
172 double computeObservationLikelihood_Consensus(
179 double computeObservationLikelihood_ConsensusOWA(
185 double computeObservationLikelihood_CellsDifference(
191 double computeObservationLikelihood_MI(
197 double computeObservationLikelihood_rayTracing(
203 double computeObservationLikelihood_likelihoodField_Thrun(
209 double computeObservationLikelihood_likelihoodField_II(
215 virtual void internal_clear( );
230 const std::vector<cellType> &
getRawMap()
const {
return this->map; }
235 void updateCell(
int x,
int y,
float v);
243 int cellsUpdated=0) : enabled(enabled),
245 cellsUpdated(cellsUpdated),
265 } updateInfoChangeOnly;
271 float min_y = -20.0f,
273 float resolution = 0.05f
278 void fill(
float default_value = 0.5f );
293 void setSize(
float x_min,
float x_max,
float y_min,
float y_max,
float resolution,
float default_value = 0.5f);
304 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;
311 inline unsigned int getSizeX()
const {
return size_x; }
315 inline unsigned int getSizeY()
const {
return size_y; }
319 inline float getXMin()
const {
return x_min; }
323 inline float getXMax()
const {
return x_max; }
327 inline float getYMin()
const {
return y_min; }
331 inline float getYMax()
const {
return y_max; }
339 inline int x2idx(
float x)
const {
return static_cast<int>((x-x_min)/resolution ); }
340 inline int y2idx(
float y)
const {
return static_cast<int>((y-y_min)/resolution ); }
342 inline int x2idx(
double x)
const {
return static_cast<int>((x-x_min)/resolution ); }
343 inline int y2idx(
double y)
const {
return static_cast<int>((y-y_min)/resolution ); }
347 inline float idx2x(
const size_t cx)
const {
return x_min+(cx+0.5f)*resolution; }
348 inline float idx2y(
const size_t cy)
const {
return y_min+(cy+0.5f)*resolution; }
352 inline int x2idx(
float x,
float x_min)
const {
return static_cast<int>((x-x_min)/resolution ); }
353 inline int y2idx(
float y,
float y_min)
const {
return static_cast<int>((y-y_min)/resolution ); }
357 static inline float l2p(
const cellType l)
359 return m_logodd_lut.
l2p(l);
364 static inline uint8_t
l2p_255(
const cellType l)
366 return m_logodd_lut.
l2p_255(l);
371 static inline cellType
p2l(
const float p)
373 return m_logodd_lut.
p2l(p);
381 if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
383 else map[x+y*size_x]=p2l(value);
391 if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
393 else return l2p(map[x+y*size_x]);
398 inline cellType *
getRow(
int cy ) {
if (cy<0 || static_cast<unsigned int>(cy)>=size_y)
return NULL;
else return &map[0+cy*size_x]; }
402 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]; }
406 inline void setPos(
float x,
float y,
float value) { setCell(x2idx(x),y2idx(y),value); }
410 inline float getPos(
float x,
float y)
const {
return getCell(x2idx(x),y2idx(y)); }
414 inline bool isStaticPos(
float x,
float y,
float threshold = 0.7f)
const {
return isStaticCell(x2idx(x),y2idx(y),threshold); }
415 inline bool isStaticCell(
int cx,
int cy,
float threshold = 0.7f)
const {
return (getCell(cx,cy)<=threshold); }
450 TEntropyInfo() : H(0),I(0),mean_H(0),mean_I(0),effectiveMappedArea(0),effectiveMappedCells(0)
500 void loadFromConfigFile(
502 const std::string §ion);
555 lmMeanInformation = 0,
584 void loadFromConfigFile(
586 const std::string §ion);
686 void subSample(
int downRatio );
706 void buildVoronoiDiagram(
float threshold,
float robot_size,
int x1=0,
int x2=0,
int y1=0,
int y2=0);
717 const uint16_t *cell=m_voronoi_diagram.
cellByIndex(cx,cy);
725 uint16_t *cell=m_voronoi_diagram.
cellByIndex(cx,cy);
748 void findCriticalPoints(
float filter_distance );
764 int computeClearance(
int cx,
int cy,
int *basis_x,
int *basis_y,
int *nBasis,
bool GetContourPoint =
false )
const;
769 float computeClearance(
float x,
float y,
float maxSearchDistance )
const;
774 float computePathCost(
float x1,
float y1,
float x2,
float y2 )
const;
795 void laserScanSimulator(
798 float threshold = 0.5f,
801 unsigned int decimation = 1,
818 float threshold = 0.5f,
819 float rangeNoiseStd = 0,
824 inline void simulateScanRay(
825 const double x,
const double y,
const double angle_direction,
826 float &out_range,
bool &out_valid,
827 const unsigned int max_ray_len,
828 const float threshold_free=0.5f,
829 const double noiseStd=0,
const double angleNoiseStd=0 )
const;
852 bool saveAsBitmapFile(
const std::string &file)
const;
858 static bool saveAsBitmapTwoMapsWithCorrespondences(
859 const std::string &fileName,
868 static bool saveAsEMFTwoMapsWithCorrespondences(
869 const std::string &fileName,
878 template <
class CLANDMARKSMAP>
880 const std::string &file,
881 const CLANDMARKSMAP *landmarks,
882 bool addTextLabels =
false,
888 getAsImageFiltered( img,
false,
true );
890 for (
unsigned int i=0;i<landmarks->landmarks.size();i++)
892 const typename CLANDMARKSMAP::landmark_type *lm = landmarks->landmarks.get( i );
893 int px = x2idx( lm->pose_mean.x );
894 int py = topleft ? size_y-1- y2idx( lm->pose_mean.y ) : y2idx( lm->pose_mean.y );
895 img.
rectangle( px - 7, (py + 7), px +7, (py -7), marks_color );
896 img.
rectangle( px - 6, (py + 6), px +6, (py -6), marks_color );
909 void getAsImage(
utils::CImage &img,
bool verticalFlip =
false,
bool forceRGB=
false,
bool tricolor =
false)
const;
915 void getAsImageFiltered(
utils::CImage &img,
bool verticalFlip =
false,
bool forceRGB=
false)
const;
919 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj )
const;
927 bool isEmpty()
const;
937 bool loadFromBitmapFile(
const std::string &file,
float resolution,
float xCentralPixel = -1,
float yCentralPixel =-1 );
947 bool loadFromBitmap(
const mrpt::utils::CImage &img,
float resolution,
float xCentralPixel = -1,
float yCentralPixel =-1 );
954 virtual void determineMatching2D(
963 float compute3DMatchingRatio(
966 float maxDistForCorr = 0.10f,
967 float maxMahaDistForCorr = 2.0f
972 void saveMetricMapRepresentationToFile(
973 const std::string &filNamePrefix
987 std::vector<int> x,
y;
993 std::vector<int> x_basis1,y_basis1, x_basis2,
y_basis2;
994 } CriticalPointsList;
1008 inline unsigned char GetNeighborhood(
int cx,
int cy )
const;
1014 int direccion_vecino_x[8],direccion_vecino_y[8];
1020 int direction2idx(
int dx,
int dy);
1024 float min_x,max_x,min_y,max_y,resolution;
1033 bool MAPS_IMPEXP operator <(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2);
float getXMax() const
Returns the "x" coordinate of right side of grid map.
float consensus_pow
[Consensus] The power factor for the likelihood (default=5)
void setCell(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
bool saveAsBitmapFileWithLandmarks(const std::string &file, const CLANDMARKSMAP *landmarks, bool addTextLabels=false, const mrpt::utils::TColor &marks_color=mrpt::utils::TColor(0, 0, 255)) const
Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel is 1 cell, and with an overlay of landmarks.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
#define ASSERT_BELOWEQ_(__A, __B)
std::vector< int > y_basis2
int32_t consensus_takeEachRange
[Consensus] The down-sample ratio of ranges (default=1, consider all the ranges)
uint16_t getVoroniClearance(int cx, int cy) const
Reads a the clearance of a cell (in centimeters), after building the Voronoi diagram with buildVorono...
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
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)...
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_)
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.
EIGEN_STRONG_INLINE void fill(const Scalar v)
float getResolution() const
Returns the resolution of the grid map.
double DEG2RAD(const double x)
Degrees to radians.
With this struct options are provided to the observation insertion process.
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...
T square(const T x)
Inline function for the square of a number.
int y2idx(float y, float y_min) const
const std::vector< cellType > & getRawMap() const
Read-only access to the raw cell contents (cells are in log-odd units)
uint16_t decimation
Specify the decimation of the range scan (default=1 : take all the range values!) ...
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.
A class for storing images as grayscale or RGB bitmaps.
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.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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
Used after member declarations.
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
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 ...
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
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)
const mrpt::utils::CDynamicGrid< uint16_t > & getVoronoiDiagram() const
Return the Voronoi diagram; each cell contains the distance to its closer obstacle, or 0 if not part of the Voronoi diagram.
std::vector< double > precomputedLikelihood
These are auxiliary variables to speed up the computation of observation likelihood values for LF met...
float MI_ratio_max_distance
[MI] The ratio for the max.
bool rayTracing_useDistanceFilter
[rayTracing] If true (default), the rayTracing method will ignore measured ranges shorter than the si...
std::vector< cellType > map
This is the buffer for storing the cells.In this dynamic size buffer are stored the cell values as "b...
const cellType * getRow(int cy) const
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally...
This class allows loading and storing values and vectors of different types from a configuration text...
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0.35
void setRawCell(unsigned int cellIndex, cellType b)
Changes a cell by its absolute index (Do not use it normally)
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation.
bool enableLikelihoodCache
Enables the usage of a cache of likelihood values (for LF methods), if set to true (default=false)...
float getCell_nocheck(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
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...
float LF_maxCorrsDistance
[LikelihoodField] The max.
float getXMin() const
Returns the "x" coordinate of left side of grid map.
With this struct options are provided to the observation likelihood computation process.
bool isStaticCell(int cx, int cy, float threshold=0.7f) const
const mrpt::utils::CDynamicGrid< uint8_t > & getBasisMap() const
Return the auxiliary "basis" map built while building the Voronoi diagram.
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
float rayTracing_stdHit
[rayTracing] The laser range sigma.
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
int y2idx(double y) const
TUpdateCellsInfoChangeOnly(bool enabled=false, double I_change=0, int cellsUpdated=0)
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...
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
static std::vector< float > entropyTable
Internally used to speed-up entropy calculation.
double I_change
The cummulative change in Information: This is updated only from the "updateCell" method...
unsigned char getBasisCell(int x, int y) const
Reads a cell in the "basis" maps.Used for Voronoi calculation.
float maxDistanceInsertion
The largest distance at which cells will be updated (Default 15 meters)
virtual void textOut(int x0, int y0, const std::string &str, const mrpt::utils::TColor color)
Renders 2D text using bitmap fonts.
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...
void setCell_nocheck(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
double mean_H
The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells) ...
bool precomputedLikelihoodToBeRecomputed
bool isStaticPos(float x, float y, float threshold=0.7f) const
Returns "true" if cell is "static", i.e.if its occupancy is below a given threshold.
An internal structure for storing data related to counting the new information apported by some obser...
A class for storing an occupancy grid map.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
bool isOriginTopLeft() const
Returns true if the coordinates origin is top-left, or false if it is bottom-left.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void setBasisCell(int x, int y, uint8_t value)
Change a cell in the "basis" maps.Used for Voronoi calculation.
#define ASSERT_ABOVEEQ_(__A, __B)
uint8_t l2p_255(const cell_t l)
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)...
unsigned long effectiveMappedCells
The mapped area in cells.
static cellType p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
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.
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, then processed by calls to this class (default=0).
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
bool m_is_empty
True upon construction; used by isEmpty()
bool useMapAltitude
The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true...
int x2idx(float x, float x_min) const
Transform a coordinate value into a cell index, using a diferent "x_min" value.
void setVoroniClearance(int cx, int cy, uint16_t dist)
Used to set the clearance of a cell, while building the Voronoi diagram.
uint32_t 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! ...
float idx2y(const size_t cy) const
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
std::vector< TPairLikelihoodIndex > OWA_pairList
[OWA method] This will contain the ascending-ordered list of pairs:(likelihood values, 2D point in map coordinates).
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0...
float voroni_free_threshold
The free-cells threshold used to compute the Voronoi diagram.
uint16_t cellTypeUnsigned
This class stores any customizable set of metric maps.
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
double mean_I
The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (ce...
Parameters for the determination of matchings between point clouds, etc.
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0...
#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:
int x2idx(float x) const
Transform a coordinate value into a cell index.
static CLogOddsGridMapLUT< cellType > m_logodd_lut
Lookup tables for log-odds.
int x2idx(double x) const
float LF_maxRange
[LikelihoodField] The max.
static uint8_t l2p_255(const cellType l)
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)...
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ...
void setPos(float x, float y, float value)
Change the contents [0,1] of a cell, given its coordinates.
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
static float l2p(const cellType l)
Scales an integer representation of the log-odd into a real valued probability in [0...
bool enabled
If set to false (default), this struct is not used.
mrpt::utils::CDynamicGrid< uint16_t > m_voronoi_diagram
Used to store the Voronoi diagram.