class mrpt::maps::COccupancyGridMap2D

Overview

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)

#include <mrpt/maps/COccupancyGridMap2D.h>

class COccupancyGridMap2D:
    public mrpt::maps::CMetricMap,
    public mrpt::maps::CLogOddsGridMap2D,
    public mrpt::maps::NearestNeighborsCapable
{
public:
    // typedefs

    typedef OccGridCellTraits::cellType cellType;
    typedef OccGridCellTraits::cellTypeUnsigned cellTypeUnsigned;
    typedef std::pair<double, mrpt::math::TPoint2D> TPairLikelihoodIndex;

    // enums

    enum TLaserSimulUncertaintyMethod;
    enum TLikelihoodMethod;

    // structs

    struct TCriticalPointsList;
    struct TEntropyInfo;
    struct TLaserSimulUncertaintyParams;
    struct TLaserSimulUncertaintyResult;
    struct TLikelihoodOutput;
    struct TUpdateCellsInfoChangeOnly;

    // classes

    class TInsertionOptions;
    class TLikelihoodOptions;

    // fields

    static constexpr cellType OCCGRID_CELLTYPE_MIN = CLogOddsGridMap2D<cellType>::CELLTYPE_MIN;
    static constexpr cellType OCCGRID_CELLTYPE_MAX = CLogOddsGridMap2D<cellType>::CELLTYPE_MAX;
    static double RAYTRACE_STEP_SIZE_IN_CELL_UNITS = 0.8;
    TUpdateCellsInfoChangeOnly updateInfoChangeOnly;
    TInsertionOptions insertionOptions;
    mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOptions;
    TLikelihoodOutput likelihoodOutputs;
    struct mrpt::maps::COccupancyGridMap2D::TCriticalPointsList CriticalPointsList;

    // construction

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

    // methods

    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;

    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;

    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;

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

    void buildVoronoiDiagram(
        float threshold,
        float robot_size,
        int x1 = 0,
        int x2 = 0,
        int y1 = 0,
        int y2 = 0
        );

    uint16_t getVoroniClearance(int cx, int cy) const;
    const mrpt::containers::CDynamicGrid<uint8_t>& getBasisMap() const;
    const mrpt::containers::CDynamicGrid<uint16_t>& getVoronoiDiagram() const;
    void findCriticalPoints(float filter_distance);
    virtual bool nn_has_indices_or_ids() const;
    virtual size_t nn_index_count() const;

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

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

    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;

    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;

    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;

    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;

    virtual void nn_prepare_for_2d_queries() const;
    virtual void nn_prepare_for_3d_queries() const;
    const std::vector<cellType>& getRawMap() const;
    void updateCell(int x, int y, float v);
    void fill(float default_value = 0.5f);

    void setSize(
        float x_min,
        float x_max,
        float y_min,
        float y_max,
        float resolution,
        float default_value = 0.5f
        );

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

    double getArea() const;
    unsigned int getSizeX() const;
    unsigned int getSizeY() const;
    float getXMin() const;
    float getXMax() const;
    float getYMin() const;
    float getYMax() const;
    float getResolution() const;
    int x2idx(float x) const;
    int y2idx(float y) const;
    int x2idx(double x) const;
    int y2idx(double y) const;
    float idx2x(size_t cx) const;
    float idx2y(size_t cy) const;
    int x2idx(float x, float xmin) const;

    int y2idx(
        float y,
        float ymin
        ) const;

    virtual mrpt::math::TBoundingBoxf boundingBox() const;
    void setCell(int x, int y, float value);
    float getCell(int x, int y) const;
    cellType* getRow(int cy);
    const cellType* getRow(int cy) const;
    void setPos(float x, float y, float value);
    float getPos(float x, float y) const;
    bool isStaticPos(float x, float y, float threshold = 0.7f) const;

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

    void setBasisCell(int x, int y, uint8_t value);
    unsigned char getBasisCell(int x, int y) const;
    void copyMapContentFrom(const COccupancyGridMap2D& otherMap);
    void subSample(int downRatio);
    void computeEntropy(TEntropyInfo& info) const;

    int computeClearance(
        int cx,
        int cy,
        int* basis_x,
        int* basis_y,
        int* nBasis,
        bool GetContourPoint = false
        ) const;

    float computeClearance(float x, float y, float maxSearchDistance) const;
    float computePathCost(float x1, float y1, float x2, float y2) const;
    double computeLikelihoodField_Thrun(const CPointsMap* pm, const mrpt::poses::CPose2D* relativePose = nullptr) const;
    double computeLikelihoodField_II(const CPointsMap* pm, const mrpt::poses::CPose2D* relativePose = nullptr) const;
    bool saveAsBitmapFile(const std::string& file) const;

    template <class CLANDMARKSMAP>
    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;

    void getAsImage(
        mrpt::img::CImage& img,
        bool verticalFlip = false,
        bool forceRGB = false,
        bool tricolor = false
        ) const;

    void getAsImageFiltered(img::CImage& img, bool verticalFlip = false, bool forceRGB = false) const;
    virtual void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const;
    void getAsPointCloud(mrpt::maps::CSimplePointsMap& pm, const float occup_threshold = 0.5f) const;
    virtual bool isEmpty() const;

    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())
        );

    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())
        );

    bool loadFromROSMapServerYAML(const std::string& yamlFilePath);

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

    virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap* otherMap, const mrpt::poses::CPose3D& otherMapPose, const TMatchingRatioParams& params) const;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    virtual std::string asString() const;
    static float l2p(const cellType l);
    static uint8_t l2p_255(const cellType l);
    static cellType p2l(const float p);

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

    static COccupancyGridMap2D FromROSMapServerYAML(const std::string& yamlFilePath);
};

Inherited Members

public:
    // typedefs

    typedef TCELL cell_t;

    // methods

    virtual bool isEmpty() const = 0;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const = 0;
    virtual std::string asString() const = 0;
    virtual void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const = 0;

    void updateCell_fast_occupied(
        const unsigned x,
        const unsigned y,
        const cell_t logodd_obs,
        const cell_t thres,
        cell_t* mapArray,
        const unsigned _size_x
        );

    void updateCell_fast_occupied(cell_t* theCell, const cell_t logodd_obs, const cell_t thres);

    static void updateCell_fast_free(
        const unsigned x,
        const unsigned y,
        const cell_t logodd_obs,
        const cell_t thres,
        cell_t* mapArray,
        const unsigned _size_x
        );

    static void updateCell_fast_free(cell_t* theCell, const cell_t logodd_obs, const cell_t thres);
    virtual bool nn_has_indices_or_ids() const = 0;
    virtual size_t nn_index_count() const = 0;

    virtual bool nn_single_search(
        const mrpt::math::TPoint3Df& query,
        mrpt::math::TPoint3Df& result,
        float& out_dist_sqr,
        uint64_t& resultIndexOrIDOrID
        ) const = 0;

    virtual bool nn_single_search(
        const mrpt::math::TPoint2Df& query,
        mrpt::math::TPoint2Df& result,
        float& out_dist_sqr,
        uint64_t& resultIndexOrIDOrID
        ) const = 0;

    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 = 0;

    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 = 0;

    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 = 0
        ) const = 0;

    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 = 0
        ) const = 0;

Typedefs

typedef OccGridCellTraits::cellType cellType

The type of the map cells:

typedef std::pair<double, mrpt::math::TPoint2D> TPairLikelihoodIndex

Auxiliary private class.

Fields

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 double RAYTRACE_STEP_SIZE_IN_CELL_UNITS = 0.8

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

TInsertionOptions insertionOptions

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

See also:

CObservation::insertIntoGridMap

Construction

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.

Methods

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.

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

See also:

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

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.

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

See also:

laserScanSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS

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.

Parameters:

in_params	[IN] Input settings. See TLaserSimulUncertaintyParams
in_params	[OUT] Output range + uncertainty.

See also:

laserScanSimulator(), COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS

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.

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.

See also:

findCriticalPoints

uint16_t getVoroniClearance(int cx, int cy) const

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

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

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

See also:

buildVoronoiDiagram

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.

Parameters:

filter_distance

The minimum distance between two critical points.

See also:

buildVoronoiDiagram

virtual bool nn_has_indices_or_ids() const

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.

virtual size_t nn_index_count() const

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& resultIndexOrIDOrID
    ) const

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

Parameters:

query	The query input point.
result	The found closest point.
out_dist_sqr	The square Euclidean distance between the query and the returned point.
resultIndexOrID	The index or ID of the result point in the map.

Returns:

True if successful, false if no point was found.

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

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

Parameters:

query	The query input point.
results	The found closest points.
out_dists_sqr	The square Euclidean distances between the query and the returned point.
resultIndicesOrIDs	The indices or IDs of the result points.

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

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

Parameters:

query	The query input point.
search_radius_sqr	The search radius, squared.
results	The found closest points.
out_dists_sqr	The square Euclidean distances between the query and the returned point.
resultIndicesOrIDs	The indices or IDs of the result points.
maxPoints	If !=0, the maximum number of neigbors to return.

virtual void nn_prepare_for_2d_queries() const

Must be called before calls to nn_*_search() to ensure the required data structures are ready for queries (e.g.

KD-trees). Useful in multithreading applications.

virtual void nn_prepare_for_3d_queries() const

Must be called before calls to nn_*_search() to ensure the required data structures are ready for queries (e.g.

KD-trees). Useful in multithreading applications.

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.

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.

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.

See also:

ResizeGrid

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
    )

Change the size of gridmap, maintaining previous contents.

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.

See also:

setSize

double getArea() const

Returns the area of the gridmap, in square meters.

unsigned int getSizeX() const

Returns the horizontal size of grid map in cells count.

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 getXMax() const

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

float getYMin() const

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

float getYMax() const

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

float getResolution() const

Returns the resolution of the grid map.

int x2idx(float x) const

Transform a coordinate value into a cell index.

float idx2x(size_t cx) const

Transform a cell index into a coordinate value.

int x2idx(float x, float xmin) const

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

virtual mrpt::math::TBoundingBoxf boundingBox() const

Returns the bounding box of the metric map, or (0,0,0)-(0,0,0) (the default value of mrpt::math::TBoundingBoxf() if not implemented in the derived class or the map is empty.

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

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

float getCell(int x, int y) const

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

cellType* getRow(int cy)

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

const cellType* getRow(int cy) const

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

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

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

float getPos(float x, float y) const

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

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.

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

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

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.

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

See also:

Build_VoronoiDiagram

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

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

virtual void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const

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.

virtual bool isEmpty() const

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

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.

See also:

loadFromBitmap

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.

Parameters:

yamlFilePath

Absolute or relative path to the .yaml file.

Returns:

false on error, true on success.

See also:

FromROSMapServerYAML()

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

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

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

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

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

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

virtual std::string asString() const

Returns a short description of the map.

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

Parameters:

yamlFilePath	Absolute or relative path to the .yaml file.
std::exception	On error loading or parsing the files.

See also:

loadFromROSMapServerYAML()