Class COccupancyGridMap2D

• Defined in File COccupancyGridMap2D.h

Nested Relationships

Nested Types

• Struct COccupancyGridMap2D::TCriticalPointsList

• Struct COccupancyGridMap2D::TEntropyInfo

• Class COccupancyGridMap2D::TInsertionOptions

• Struct COccupancyGridMap2D::TLaserSimulUncertaintyParams

• Struct COccupancyGridMap2D::TLaserSimulUncertaintyResult

• Class COccupancyGridMap2D::TLikelihoodOptions

• Struct COccupancyGridMap2D::TLikelihoodOutput

• Struct COccupancyGridMap2D::TUpdateCellsInfoChangeOnly

Inheritance Relationships

Base Types

• public mrpt::maps::CMetricMap

• public mrpt::maps::CLogOddsGridMap2D< OccGridCellTraits::cellType > (Template Struct CLogOddsGridMap2D)

• public mrpt::maps::NearestNeighborsCapable (Class NearestNeighborsCapable)

Class Documentation

class COccupancyGridMap2D : public mrpt::maps::CMetricMap, public mrpt::maps::CLogOddsGridMap2D<OccGridCellTraits::cellType>, public mrpt::maps::NearestNeighborsCapable

A class for storing an occupancy grid map. COccupancyGridMap2D is a class for storing a metric map representation in the form of a probabilistic occupancy grid map: value of 0 means certainly occupied, 1 means a certainly empty cell. Initially 0.5 means uncertainty.

The cells keep the log-odd representation of probabilities instead of the probabilities themselves. More details can be found at https://www.mrpt.org/Occupancy_Grids

The algorithm for updating the grid from a laser scanner can optionally take into account the progressive widening of the beams, as described in this page

Some implemented methods are:

• Update of individual cells

• Insertion of observations

• Voronoi diagram and critical points (buildVoronoiDiagram)

• Saving and loading from/to a bitmap

• Laser scans simulation for the map contents

• Entropy and information methods (See computeEntropy)

Sensor simulators

enum TLaserSimulUncertaintyMethod

Methods for TLaserSimulUncertaintyParams in laserScanSimulatorWithUncertainty()

Values:

enumerator sumUnscented

Performs an unscented transform

enumerator sumMonteCarlo

Montecarlo-based estimation

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::DEG2RAD(.0)) const

Simulates a laser range scan into the current grid map. The simulated scan is stored in a CObservation2DRangeScan object, which is also used to pass some parameters: all previously stored characteristics (as aperture,…) are taken into account for simulation. Only a few more parameters are needed. Additive gaussian noise can be optionally added to the simulated scan.

See also

laserScanSimulatorWithUncertainty(), sonarSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS

Parameters:

• inout_Scan – [IN/OUT] This must be filled with desired parameters before calling, and will contain the scan samples on return.

• 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.

• threshold – [IN] The minimum occupancy threshold to consider a cell to be occupied (Default: 0.5f)

• N – [IN] The count of range scan “rays”, by default to 361.

• noiseStd – [IN] The standard deviation of measurement noise. If not desired, set to 0.

• decimation – [IN] The rays that will be simulated are at indexes: 0, D, 2D, 3D, … Default is D=1

• angleNoiseStd – [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians).

void sonarSimulator(mrpt::obs::CObservationRange &inout_observation, const mrpt::poses::CPose2D &robotPose, float threshold = 0.5f, float rangeNoiseStd = 0.f, float angleNoiseStd = mrpt::DEG2RAD(0.f)) const

Simulates the observations of a sonar rig into the current grid map. The simulated ranges are stored in a CObservationRange object, which is also used to pass in some needed parameters, as the poses of the sonar sensors onto the mobile robot.

See also

laserScanSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS

Parameters:

• inout_observation – [IN/OUT] This must be filled with desired parameters before calling, and will contain the simulated ranges on return.

• 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.

• threshold – [IN] The minimum occupancy threshold to consider a cell to be occupied (Default: 0.5f)

• rangeNoiseStd – [IN] The standard deviation of measurement noise. If not desired, set to 0.

• angleNoiseStd – [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians).

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. This method is used internally to sonarSimulator and laserScanSimulator.

See also

COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS

void laserScanSimulatorWithUncertainty(const TLaserSimulUncertaintyParams &in_params, TLaserSimulUncertaintyResult &out_results) const

Like laserScanSimulatorWithUncertainty() (see it for a discussion of most parameters) but taking into account the robot pose uncertainty and generating a vector of predicted variances for each ray. Range uncertainty includes both, sensor noise and large non-linear effects caused by borders and discontinuities in the environment as seen from different robot poses.

See also

laserScanSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS

Parameters:

• in_params – [IN] Input settings. See TLaserSimulUncertaintyParams

• in_params – [OUT] Output range + uncertainty.

Voronoi methods

inline void setVoroniClearance(int cx, int cy, uint16_t dist)

Used to set the clearance of a cell, while building the Voronoi diagram.

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.

See also

findCriticalPoints

Parameters:

• threshold – The threshold for binarizing the map.

• robot_size – Size in “units” (meters) of robot, approx.

• x1 – Left coordinate of area to be computed. Default, entire map.

• x2 – Right coordinate of area to be computed. Default, entire map.

• y1 – Top coordinate of area to be computed. Default, entire map.

• y2 – Bottom coordinate of area to be computed. Default, entire map.

inline uint16_t getVoroniClearance(int cx, int cy) const

Reads a the clearance of a cell (in centimeters), after building the Voronoi diagram with buildVoronoiDiagram

inline const mrpt::containers::CDynamicGrid<uint8_t> &getBasisMap() const

Return the auxiliary “basis” map built while building the Voronoi diagram

See also

buildVoronoiDiagram

inline const mrpt::containers::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

See also

buildVoronoiDiagram

void findCriticalPoints(float filter_distance)

Builds a list with the critical points from Voronoi diagram, which must must be built before calling this method.

See also

buildVoronoiDiagram

Parameters:

filter_distance – The minimum distance between two critical points.

API of the NearestNeighborsCapable virtual interface

inline virtual bool nn_has_indices_or_ids() const override

Returns true if the rest of nn_* methods will populate the output indices values with 0-based contiguous indices. Returns false if indices are actually sparse ID numbers without any expectation of they be contiguous or start near zero.

inline virtual size_t nn_index_count() const override

If nn_has_indices_or_ids() returns true, this must return the number of “points” (or whatever entity) the indices correspond to. Otherwise, the return value should be ignored.

virtual bool nn_single_search(const mrpt::math::TPoint3Df &query, mrpt::math::TPoint3Df &result, float &out_dist_sqr, uint64_t &resultIndexOrID) const override

Search for the closest 3D point to a given one.

Parameters:

• query – [in] The query input point.

• result – [out] The found closest point.

• out_dist_sqr – [out] The square Euclidean distance between the query and the returned point.

• resultIndexOrID – [out] The index or ID of the result point in the map.

Returns:

True if successful, false if no point was found.

virtual bool nn_single_search(const mrpt::math::TPoint2Df &query, mrpt::math::TPoint2Df &result, float &out_dist_sqr, uint64_t &resultIndexOrID) const override

virtual void nn_multiple_search(const mrpt::math::TPoint3Df &query, const size_t N, std::vector<mrpt::math::TPoint3Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs) const override

Search for the N closest 3D points to a given one.

Parameters:

• query – [in] The query input point.

• results – [out] The found closest points.

• out_dists_sqr – [out] The square Euclidean distances between the query and the returned point.

• resultIndicesOrIDs – [out] The indices or IDs of the result points.

virtual void nn_multiple_search(const mrpt::math::TPoint2Df &query, const size_t N, std::vector<mrpt::math::TPoint2Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs) const override

virtual void nn_radius_search(const mrpt::math::TPoint3Df &query, const float search_radius_sqr, std::vector<mrpt::math::TPoint3Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs, size_t maxPoints) const override

Radius search for closest 3D points to a given one.

Parameters:

• query – [in] The query input point.

• search_radius_sqr – [in] The search radius, squared.

• results – [out] The found closest points.

• out_dists_sqr – [out] The square Euclidean distances between the query and the returned point.

• resultIndicesOrIDs – [out] The indices or IDs of the result points.

• maxPoints – [in] If !=0, the maximum number of neigbors to return.

virtual void nn_radius_search(const mrpt::math::TPoint2Df &query, const float search_radius_sqr, std::vector<mrpt::math::TPoint2Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs, size_t maxPoints) const override

Public Types

enum TLikelihoodMethod

The type for selecting a likelihood computation method

Values:

enumerator lmMeanInformation

enumerator lmRayTracing

enumerator lmConsensus

enumerator lmCellsDifference

enumerator lmLikelihoodField_Thrun

enumerator lmLikelihoodField_II

enumerator lmConsensusOWA

using cellType = OccGridCellTraits::cellType

The type of the map cells:

using cellTypeUnsigned = OccGridCellTraits::cellTypeUnsigned

using TPairLikelihoodIndex = std::pair<double, mrpt::math::TPoint2D>

Auxiliary private class.

Public Functions

inline const std::vector<cellType> &getRawMap() const

Read-only access to the raw cell contents (cells are in log-odd units)

void updateCell(int x, int y, float v)

Performs the Bayesian fusion of a new observation of a cell

See also

updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free

void fill(float default_value = 0.5f)

Fills all the cells with a default value.

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

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.

See also

ResizeGrid

Parameters:

• x_min – The “x” coordinates of left most side of grid.

• x_max – The “x” coordinates of right most side of grid.

• y_min – The “y” coordinates of top most side of grid.

• y_max – The “y” coordinates of bottom most side of grid.

• resolution – The new size of cells.

• default_value – The value of cells, typically 0.5.

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) noexcept

Change the size of gridmap, maintaining previous contents.

See also

setSize

Parameters:

• new_x_min – The “x” coordinates of new left most side of grid.

• new_x_max – The “x” coordinates of new right most side of grid.

• new_y_min – The “y” coordinates of new top most side of grid.

• new_y_max – The “y” coordinates of new bottom most side of grid.

• new_cells_default_value – The value of the new cells, typically 0.5.

• 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.

inline double getArea() const

Returns the area of the gridmap, in square meters

inline unsigned int getSizeX() const

Returns the horizontal size of grid map in cells count

inline unsigned int getSizeY() const

Returns the vertical size of grid map in cells count

inline float getXMin() const

Returns the “x” coordinate of left side of grid map

inline float getXMax() const

Returns the “x” coordinate of right side of grid map

inline float getYMin() const

Returns the “y” coordinate of top side of grid map

inline float getYMax() const

Returns the “y” coordinate of bottom side of grid map

inline float getResolution() const

Returns the resolution of the grid map

inline int x2idx(float x) const

Transform a coordinate value into a cell index

inline int y2idx(float y) const

inline int x2idx(double x) const

inline int y2idx(double y) const

inline float idx2x(size_t cx) const

Transform a cell index into a coordinate value

inline float idx2y(size_t cy) const

inline int x2idx(float x, float xmin) const

Transform a coordinate value into a cell index, using a different “x_min” value

inline int y2idx(float y, float ymin) const

inline mrpt::math::TBoundingBoxf boundingBox() const override

inline void setCell(int x, int y, float value)

Change the contents [0,1] of a cell, given its index

inline float getCell(int x, int y) const

Read the real valued [0,1] contents of a cell, given its index

inline cellType *getRow(int cy)

Access to a “row”: mainly used for drawing grid as a bitmap efficiently, do not use it normally

inline const cellType *getRow(int cy) const

Access to a “row”: mainly used for drawing grid as a bitmap efficiently, do not use it normally

inline void setPos(float x, float y, float value)

Change the contents [0,1] of a cell, given its coordinates

inline float getPos(float x, float y) const

Read the real valued [0,1] contents of a cell, given its coordinates

inline 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

inline bool isStaticCell(int cx, int cy, float threshold = 0.7f) const

inline void setBasisCell(int x, int y, uint8_t value)

Change a cell in the “basis” maps.Used for Voronoi calculation

inline unsigned char getBasisCell(int x, int y) const

Reads a cell in the “basis” maps.Used for Voronoi calculation

void copyMapContentFrom(const COccupancyGridMap2D &otherMap)

copy the gridmap contents, but not all the options, from another map instance

void subSample(int downRatio)

Performs a downsampling of the gridmap, by a given factor: resolution/=ratio

void computeEntropy(TEntropyInfo &info) const

Computes the entropy and related values of this grid map. The entropy is computed as the summed entropy of each cell, taking them as discrete random variables following a Bernoulli distribution:

Parameters:

info – The output information is returned here

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.Used to build Voronoi and critical points.

See also

Build_VoronoiDiagram

Parameters:

• cx – The cell index

• cy – The cell index

• basis_x – Target buffer for coordinates of basis, having a size of two “ints”.

• basis_y – Target buffer for coordinates of basis, having a size of two “ints”.

• nBasis – The number of found basis: Can be 0,1 or 2.

• GetContourPoint – If “true” the basis are not returned, but the closest free cells.Default at false.

Returns:

The clearance of the cell, in 1/100 of “cell”.

float computeClearance(float x, float y, float maxSearchDistance) const

An alternative method for computing the clearance of a given location (in meters).

Returns:

The clearance (distance to closest OCCUPIED cell), in meters.

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.

Returns:

This returns ‘1-mean(traversed cells occupancy)’, i.e. 0.5 for unknown cells, 1 for a free path.

double computeLikelihoodField_Thrun(const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose = nullptr) const

Computes the likelihood [0,1] of a set of points, given the current grid map as reference.

Parameters:

• pm – The points map

• relativePose – The relative pose of the points map in this map’s coordinates, or nullptr for (0,0,0). See “likelihoodOptions” for configuration parameters.

double computeLikelihoodField_II(const CPointsMap *pm, const mrpt::poses::CPose2D *relativePose = nullptr) const

Computes the likelihood [0,1] of a set of points, given the current grid map as reference.

Parameters:

• pm – The points map

• relativePose – The relative pose of the points map in this map’s coordinates, or nullptr for (0,0,0). See “likelihoodOptions” for configuration parameters.

bool saveAsBitmapFile(const std::string &file) const

Saves the gridmap as a graphical file (BMP,PNG,…). The format will be derived from the file extension (see CImage::saveToFile )

Returns:

False on any error.

template<class CLANDMARKSMAP>
inline bool saveAsBitmapFileWithLandmarks(const std::string &file, const CLANDMARKSMAP *landmarks, bool addTextLabels = false, const mrpt::img::TColor &marks_color = mrpt::img::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.

Note

The template parameter CLANDMARKSMAP is assumed to be mrpt::maps::CLandmarksMap normally.

Returns:

False on any error.

void getAsImage(mrpt::img::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 forceRGB is true) 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.

See also

getAsImageFiltered

void getAsImageFiltered(img::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 forceRGB is true) - This method filters the image for easy feature detection 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.

See also

getAsImage

void getVisualizationInto(mrpt::viz::CSetOfObjects &outObj) const override

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)

void getAsPointCloud(mrpt::maps::CSimplePointsMap &pm, const float occup_threshold = 0.5f) const

Get a point cloud with all (border) occupied cells as points

bool isEmpty() const override

Returns true upon map construction or after calling clear(), the return changes to false upon successful insertObservation() or any other method to load data in the map.

bool loadFromBitmapFile(const std::string &file, float resolution, const mrpt::math::TPoint2D &origin = mrpt::math::TPoint2D(std::numeric_limits<double>::max(), std::numeric_limits<double>::max()))

Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).

See also

loadFromBitmap

Parameters:

• file – The file to be loaded.

• resolution – The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed.

• origin – The x (0=first, increases left to right on the image) and y (0=first, increases BOTTOM upwards on the image) coordinates for the pixel which will be taken at the origin of map coordinates (0,0). (Default=center of the image)

Returns:

False on any error.

bool loadFromBitmap(const mrpt::img::CImage &img, float resolution, const mrpt::math::TPoint2D &origin = mrpt::math::TPoint2D(std::numeric_limits<double>::max(), std::numeric_limits<double>::max()))

Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile). See loadFromBitmapFile() for the meaning of parameters

bool loadFromROSMapServerYAML(const std::string &yamlFilePath)

Loads this gridmap from a .yaml file and an accompanying image file given in the map_server YAML file format.

See also

FromROSMapServerYAML()

Parameters:

yamlFilePath – Absolute or relative path to the .yaml file.

Returns:

false on error, true on success.

void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override

See the base class for more details: In this class it is implemented as correspondences of the passed points map to occupied cells. NOTICE: That the “z” dimension is ignored in the points. Clip the points as appropiated if needed before calling this method.

See also

computeMatching3DWith

float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override

See docs in base class: in this class this always returns 0

void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override

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).

inline std::string asString() const override

Returns a short description of the map.

Public Members

mutable TUpdateCellsInfoChangeOnly updateInfoChangeOnly

TInsertionOptions insertionOptions

With this struct options are provided to the observation insertion process

See also

CObservation::insertIntoGridMap

mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOptions

mutable TLikelihoodOutput likelihoodOutputs

struct mrpt::maps::COccupancyGridMap2D::TCriticalPointsList CriticalPointsList

Public Static Functions

static inline float l2p(const cellType l)

Scales an integer representation of the log-odd into a real valued probability in [0,1], using p=exp(l)/(1+exp(l))

static inline 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 inline 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 valid range of cellType

static bool saveAsBitmapTwoMapsWithCorrespondences(const std::string &fileName, const COccupancyGridMap2D *m1, const COccupancyGridMap2D *m2, const mrpt::tfest::TMatchingPairList &corrs)

Saves a composite image with two gridmaps and lines representing a set of correspondences between them.

Returns:

False on any error.

static inline COccupancyGridMap2D FromROSMapServerYAML(const std::string &yamlFilePath)

Creates a gridmap from a .yaml file and an accompanying image file given in the map_server YAML file format.

See also

loadFromROSMapServerYAML()

Parameters:

yamlFilePath – Absolute or relative path to the .yaml file.

Throws:

std::exception – On error loading or parsing the files.

Public Static Attributes

static constexpr cellType OCCGRID_CELLTYPE_MIN = CLogOddsGridMap2D<cellType>::CELLTYPE_MIN

Discrete to float conversion factors: The min/max values of the integer cell type, eg.[0,255] or [0,65535]

static constexpr cellType OCCGRID_CELLTYPE_MAX = CLogOddsGridMap2D<cellType>::CELLTYPE_MAX

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.

Protected Functions

void freeMap()

Frees the dynamic memory buffers of map.

void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation&) override

See base class

inline void setCell_nocheck(int x, int y, float value)

Change the contents [0,1] of a cell, given its index

inline float getCell_nocheck(int x, int y) const

Read the real valued [0,1] contents of a cell, given its index

inline void setRawCell(unsigned int cellIndex, cellType b)

Changes a cell by its absolute index (Do not use it normally)

double computeObservationLikelihood_Consensus(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose2D &takenFrom) const

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.)

double computeObservationLikelihood_ConsensusOWA(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose2D &takenFrom) const

One of the methods that can be selected for implementing “computeObservationLikelihood”. TODO: This method is described in….

double computeObservationLikelihood_CellsDifference(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose2D &takenFrom) const

One of the methods that can be selected for implementing “computeObservationLikelihood”

double computeObservationLikelihood_MI(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose2D &takenFrom) const

One of the methods that can be selected for implementing “computeObservationLikelihood”

double computeObservationLikelihood_rayTracing(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose2D &takenFrom) const

One of the methods that can be selected for implementing “computeObservationLikelihood”

double computeObservationLikelihood_likelihoodField_Thrun(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose2D &takenFrom) const

One of the methods that can be selected for implementing “computeObservationLikelihood”.

double computeObservationLikelihood_likelihoodField_II(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose2D &takenFrom) const

One of the methods that can be selected for implementing “computeObservationLikelihood”.

void internal_clear() 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).

bool internal_insertObservation(const mrpt::obs::CObservation &obs, const std::optional<const mrpt::poses::CPose3D> &robotPose) override

Insert the observation information into this map.

After successful execution, “lastObservationInsertionInfo” is updated.

See also

insertionOptions, CObservation::insertObservationInto

Parameters:

• obs – The observation

• robotPose – The 3D pose of the robot mobile base in the map reference system, or nullptr (default) if you want to use CPose2D(0,0,deg)

Protected Attributes

std::vector<cellType> m_map

Store of cell occupancy values. Order: row by row, from left to right

uint32_t m_size_x = 0

The size of the grid in cells

uint32_t m_size_y = 0

float m_xMin = 0

The limits of the grid in “units” (meters)

float m_xMax = 0

float m_yMin = 0

float m_yMax = 0

float m_resolution = 0

Cell size, i.e. resolution of the grid map.

mutable mrpt::containers::NonCopiableData<std::vector<std::atomic<double>>> m_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).

mutable bool m_likelihoodCacheOutDated = {true}

mutable mrpt::containers::NonCopiableData<std::mutex> m_precomputedLikelihood_mtx

mrpt::containers::CDynamicGrid<uint8_t> m_basis_map

Used for Voronoi calculation.Same struct as “map”, but contains a “0” if not a basis point.

mrpt::containers::CDynamicGrid<uint16_t> m_voronoi_diagram

Used to store the Voronoi diagram. Contains the distance of each cell to its closer obstacles in 1/100th distance units (i.e. in centimeters), or 0 if not into the Voronoi diagram

bool m_is_empty = {true}

True upon construction; used by isEmpty()

float voroni_free_threshold = {}

The free-cells threshold used to compute the Voronoi diagram.

Protected Static Functions

static CLogOddsGridMapLUT<cellType> &get_logodd_lut()

Lookup tables for log-odds

template<typename T>
static inline T H(const T p)

Entropy computation internal function:

Protected Static Attributes

static std::vector<float> entropyTable

Internally used to speed-up entropy calculation

Friends

friend class CMultiMetricMap

friend class CMultiMetricMapPDF

struct TCriticalPointsList

The structure used to store the set of Voronoi diagram critical points.

See also

findCriticalPoints

Public Functions

inline TCriticalPointsList()

Public Members

std::vector<int> x

The coordinates of critical point.

std::vector<int> y

std::vector<int> clearance

The clearance of critical points, in 1/100 of cells.

std::vector<int> x_basis1

Their two first basis points coordinates.

std::vector<int> y_basis1

std::vector<int> x_basis2

std::vector<int> y_basis2

struct TEntropyInfo

Used for returning entropy related information

See also

computeEntropy

Public Members

double H = {0}

The target variable for absolute entropy, computed as:H(map)=Sum{ -p(x,y)*ln(p(x,y)) -(1-p(x,y))*ln(1-p(x,y)) }

double I = {0}

The target variable for absolute “information”, defining I(x) = 1 - H(x)

double mean_H = {0}

The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells)

double mean_I = {0}

The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (cells)

double effectiveMappedArea = {0}

The target variable for the area of cells with information, i.e. p(x)!=0.5

unsigned long effectiveMappedCells = {0}

The mapped area in cells.

class TInsertionOptions : public mrpt::config::CLoadableOptions

With this struct options are provided to the observation insertion process.

See also

CObservation::insertIntoGridMap

Public Functions

TInsertionOptions() = default

void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override

This method load the options from a “.ini” file. Only those parameters found in the given “section” and having the same name that the variable are loaded. Those not found in the file will stay with their previous values (usually the default values loaded at initialization). An example of an “.ini” file:

[section]
resolution=0.10     ; blah blah...
modeSelection=1     ; 0=blah, 1=blah,...

void dumpToTextStream(std::ostream &out) const override

Public Members

float mapAltitude = {0}

The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map!

bool useMapAltitude = {false}

The parameter “mapAltitude” has effect while inserting observations in the grid only if this is true.

float maxDistanceInsertion = {15.0f}

The largest distance at which cells will be updated (Default 15 meters)

float maxOccupancyUpdateCertainty = {0.65f}

A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0.8)

float maxFreenessUpdateCertainty = {.0f}

A value in the range [0.5,1] for updating a free cell. (default=0 means use the same than maxOccupancyUpdateCertainty)

float maxFreenessInvalidRanges = {.0f}

Like maxFreenessUpdateCertainty, but for invalid ranges (no echo). (default=0 means same than maxOccupancyUpdateCertainty)

bool considerInvalidRangesAsFreeSpace = {true}

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.

uint16_t decimation = {1}

Specify the decimation of the range scan (default=1 : take all the range values!)

float horizontalTolerance = {mrpt::DEG2RAD(0.05f)}

The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, then processed by calls to this class (default=0).

float CFD_features_gaussian_size = {1}

Gaussian sigma of the filter used in getAsImageFiltered (for features detection) (Default=1) (0:Disabled)

float CFD_features_median_size = {3}

Size of the Median filter used in getAsImageFiltered (for features detection) (Default=3) (0:Disabled)

bool wideningBeamsWithDistance = {false}

Enabled: Rays widen with distance to approximate the real behavior of lasers, disabled: insert rays as simple lines (Default=false)

struct TLaserSimulUncertaintyParams

Input params for laserScanSimulatorWithUncertainty()

Parameters for each uncertainty method

double UT_alpha = {0.99}

[sumUnscented] UT parameters. Defaults: alpha=0.99, kappa=0, betta=2.0

double UT_kappa = {.0}

double UT_beta = {2.0}

size_t MC_samples = {10}

[sumMonteCarlo] MonteCarlo parameter: number of samples (Default: 10)

Generic parameters for all methods

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

float aperture = {M_PIf}

(Default: M_PI) The “aperture” or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).

bool rightToLeft = {true}

(Default: true) The scanning direction: true=counterclockwise; false=clockwise

float maxRange = {80.f}

(Default: 80) The maximum range allowed by the device, in meters (e.g. 80m, 50m,…)

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 nRays = {361}

float rangeNoiseStd = {.0f}

(Default: 0) The standard deviation of measurement noise. If not desired, set to 0

float angleNoiseStd = {.0f}

(Default: 0) The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians)

unsigned int decimation = {1}

(Default: 1) The rays that will be simulated are at indexes: 0, D, 2D, 3D,…

float threshold = {.6f}

(Default: 0.6f) The minimum occupancy threshold to consider a cell to be occupied

Public Functions

TLaserSimulUncertaintyParams()

Public Members

TLaserSimulUncertaintyMethod method = {sumUnscented}

(Default: sumMonteCarlo) Select the method to do the uncertainty propagation

struct TLaserSimulUncertaintyResult

Output params for laserScanSimulatorWithUncertainty()

Public Functions

TLaserSimulUncertaintyResult()

Public Members

mrpt::obs::CObservation2DRangeScanWithUncertainty scanWithUncert

The scan + its uncertainty

class TLikelihoodOptions : public mrpt::config::CLoadableOptions

With this struct options are provided to the observation likelihood computation process

Public Functions

TLikelihoodOptions()

Initialization of default parameters

void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override

This method load the options from a “.ini” file. Only those parameters found in the given “section” and having the same name that the variable are loaded. Those not found in the file will stay with their previous values (usually the default values loaded at initialization). An example of an “.ini” file:

[section]
resolution=0.10     ; blah blah...
modeSelection=1     ; 0=blah, 1=blah,...

void dumpToTextStream(std::ostream &out) const override

Public Members

TLikelihoodMethod likelihoodMethod = {lmLikelihoodField_Thrun}

The selected method to compute an observation likelihood

float LF_stdHit = {0.35f}

[LikelihoodField] The laser range “sigma” used in computations; Default value = 0.35

float LF_zHit = {0.95f}

[LikelihoodField]

float LF_zRandom = {0.05f}

float LF_maxRange = {81.0f}

[LikelihoodField] The max. range of the sensor (Default= 81 m)

uint32_t LF_decimation = {5}

[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation

float LF_maxCorrsDistance = {0.3f}

[LikelihoodField] The max. distance for searching correspondences around each sensed point

bool LF_useSquareDist = {false}

[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2)

bool LF_alternateAverageMethod = {false}

[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”.

float MI_exponent = {2.5f}

[MI] The exponent in the MI likelihood computation. Default value = 5

uint32_t MI_skip_rays = {10}

[MI] The scan rays decimation: at every N rays, one will be used to compute the MI

float MI_ratio_max_distance = {1.5f}

[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.

bool rayTracing_useDistanceFilter = {true}

[rayTracing] If true (default), the rayTracing method will ignore measured ranges shorter than the simulated ones.

int32_t rayTracing_decimation = {10}

[rayTracing] One out of “rayTracing_decimation” rays will be simulated and compared only: set to 1 to use all the sensed ranges.

float rayTracing_stdHit = {1.0f}

[rayTracing] The laser range sigma.

int32_t consensus_takeEachRange = {1}

[Consensus] The down-sample ratio of ranges (default=1, consider all the ranges)

float consensus_pow = {5}

[Consensus] The power factor for the likelihood (default=5)

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.

bool enableLikelihoodCache = {true}

Enables the usage of a cache of likelihood values (for LF methods), if set to true (default=false).

struct TLikelihoodOutput

Some members of this struct will contain intermediate or output data after calling “computeObservationLikelihood” for some likelihood functions

Public Members

std::vector<TPairLikelihoodIndex> OWA_pairList

[OWA method] This will contain the ascending-ordered list of pairs:(likelihood values, 2D point in map coordinates).

std::vector<double> OWA_individualLikValues

[OWA method] This will contain the ascending-ordered list of likelihood values for individual range measurements in the scan.

struct TUpdateCellsInfoChangeOnly

An internal structure for storing data related to counting the new information apported by some observation

Public Functions

TUpdateCellsInfoChangeOnly() = default

Public Members

bool enabled = {false}

If set to false (default), this struct is not used. Set to true only when measuring the info of an observation.

double I_change = {0}

The cummulative change in Information: This is updated only from the “updateCell” method.

int cellsUpdated = {0}

The cummulative updated cells count: This is updated only from the “updateCell” method.

int laserRaysSkip = {1}

In this mode, some laser rays can be skips to speep-up