class mrpt::maps::CColouredOctoMap

Overview

A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the “octomap” C++ library.

This version stores both, occupancy information and RGB colour data at each octree node. See the base class mrpt::maps::COctoMapBase.

The octomap library was presented in [8]

See also:

CMetricMap, the example in “MRPT/samples/octomap_simple”

#include <mrpt/maps/CColouredOctoMap.h>

class CColouredOctoMap: public mrpt::maps::COctoMapBase
{
public:
    // enums

    enum TColourUpdate;

    // fields

    double resolution {      0.10};
    mrpt::maps::CColouredOctoMap::TInsertionOptions insertionOpts;
    mrpt::maps::CColouredOctoMap::TLikelihoodOptions likelihoodOpts;
    TInsertionOptions insertionOptions;

    // construction

    CColouredOctoMap(const double resolution = 0.10);

    // methods

    void insertRay(
        const float end_x,
        const float end_y,
        const float end_z,
        const float sensor_x,
        const float sensor_y,
        const float sensor_z
        );

    void updateVoxel(const double x, const double y, const double z, bool occupied);
    bool isPointWithinOctoMap(const float x, const float y, const float z) const;
    double getResolution() const;
    unsigned int getTreeDepth() const;
    size_t size() const;
    size_t memoryUsage() const;
    size_t memoryUsageNode() const;
    size_t memoryFullGrid() const;
    double volume();
    void getMetricSize(double& x, double& y, double& z);
    void getMetricSize(double& x, double& y, double& z) const;
    void getMetricMin(double& x, double& y, double& z);
    void getMetricMin(double& x, double& y, double& z) const;
    void getMetricMax(double& x, double& y, double& z);
    void getMetricMax(double& x, double& y, double& z) const;
    size_t calcNumNodes() const;
    size_t getNumLeafNodes() const;
    virtual void setOccupancyThres(double prob);
    virtual void setProbHit(double prob);
    virtual void setProbMiss(double prob);
    virtual void setClampingThresMin(double thresProb);
    virtual void setClampingThresMax(double thresProb);
    virtual double getOccupancyThres() const;
    virtual float getOccupancyThresLog() const;
    virtual double getProbHit() const;
    virtual float getProbHitLog() const;
    virtual double getProbMiss() const;
    virtual float getProbMissLog() const;
    virtual double getClampingThresMin() const;
    virtual float getClampingThresMinLog() const;
    virtual double getClampingThresMax() const;
    virtual float getClampingThresMaxLog() const;

    bool getPointColour(
        const float x,
        const float y,
        const float z,
        uint8_t& r,
        uint8_t& g,
        uint8_t& b
        ) const;

    void updateVoxelColour(
        const double x,
        const double y,
        const double z,
        const uint8_t r,
        const uint8_t g,
        const uint8_t b
        );

    void setVoxelColourMethod(TColourUpdate new_method);
    TColourUpdate getVoxelColourMethod();
    virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const;
    virtual bool isEmpty() const;
    OCTOMAP_CLASS& getOctomap();
    virtual std::string asString() const;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    virtual void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const;
    bool getPointOccupancy(const float x, const float y, const float z, double& prob_occupancy) const;

    void insertPointCloud(
        const CPointsMap& ptMap,
        const float sensor_x,
        const float sensor_y,
        const float sensor_z
        );

    bool castRay(
        const mrpt::math::TPoint3D& origin,
        const mrpt::math::TPoint3D& direction,
        mrpt::math::TPoint3D& end,
        bool ignoreUnknownCells = false,
        double maxRange = -1.0
        ) const;

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

Inherited Members

public:
    // typedefs

    typedef COctoMapBase<octree_t, octree_node_t> myself_t;

    // structs

    struct TInsertionOptions;
    struct TLikelihoodOptions;
    struct TRenderingOptions;

    // fields

    TLikelihoodOptions likelihoodOptions;
    TRenderingOptions renderingOptions;

    // 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;
    virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const = 0;
    virtual void setOccupancyThres(double prob) = 0;
    virtual void setProbHit(double prob) = 0;
    virtual void setProbMiss(double prob) = 0;
    virtual void setClampingThresMin(double thresProb) = 0;
    virtual void setClampingThresMax(double thresProb) = 0;
    virtual double getOccupancyThres() const = 0;
    virtual float getOccupancyThresLog() const = 0;
    virtual double getProbHit() const = 0;
    virtual float getProbHitLog() const = 0;
    virtual double getProbMiss() const = 0;
    virtual float getProbMissLog() const = 0;
    virtual double getClampingThresMin() const = 0;
    virtual float getClampingThresMinLog() const = 0;
    virtual double getClampingThresMax() const = 0;
    virtual float getClampingThresMaxLog() const = 0;

Fields

double resolution {         0.10}

The finest resolution of the octomap (default: 0.10.

mrpt::maps::CColouredOctoMap::TInsertionOptions insertionOpts

meters)

Observations insertion options

mrpt::maps::CColouredOctoMap::TLikelihoodOptions likelihoodOpts

Probabilistic observation likelihood options.

TInsertionOptions insertionOptions

The options used when inserting.

Construction

CColouredOctoMap(const double resolution = 0.10)

Default constructor.

Methods

void insertRay(
    const float end_x,
    const float end_y,
    const float end_z,
    const float sensor_x,
    const float sensor_y,
    const float sensor_z
    )

Just like insertPointCloud but with a single ray.

void updateVoxel(const double x, const double y, const double z, bool occupied)

Manually updates the occupancy of the voxel at (x,y,z) as being occupied (true) or free (false), using the log-odds parameters in insertionOptions.

bool isPointWithinOctoMap(const float x, const float y, const float z) const

Check whether the given point lies within the volume covered by the octomap (that is, whether it is “mapped”)

size_t size() const

Returns:

The number of nodes in the tree

size_t memoryUsage() const

Returns:

Memory usage of the complete octree in bytes (may vary between architectures)

size_t memoryUsageNode() const

Returns:

Memory usage of the a single octree node

size_t memoryFullGrid() const

Returns:

Memory usage of a full grid of the same size as the OcTree in bytes (for comparison)

void getMetricSize(double& x, double& y, double& z)

Size of OcTree (all known space) in meters for x, y and z dimension.

void getMetricSize(double& x, double& y, double& z) const

Size of OcTree (all known space) in meters for x, y and z dimension.

void getMetricMin(double& x, double& y, double& z)

minimum value of the bounding box of all known space in x, y, z

void getMetricMin(double& x, double& y, double& z) const

minimum value of the bounding box of all known space in x, y, z

void getMetricMax(double& x, double& y, double& z)

maximum value of the bounding box of all known space in x, y, z

void getMetricMax(double& x, double& y, double& z) const

maximum value of the bounding box of all known space in x, y, z

size_t calcNumNodes() const

Traverses the tree to calculate the total number of nodes.

size_t getNumLeafNodes() const

Traverses the tree to calculate the total number of leaf nodes.

bool getPointColour(
    const float x,
    const float y,
    const float z,
    uint8_t& r,
    uint8_t& g,
    uint8_t& b
    ) const

Get the RGB colour of a point.

Returns:

false if the point is not mapped, in which case the returned colour is undefined.

void updateVoxelColour(
    const double x,
    const double y,
    const double z,
    const uint8_t r,
    const uint8_t g,
    const uint8_t b
    )

Manually update the colour of the voxel at (x,y,z)

void setVoxelColourMethod(TColourUpdate new_method)

Set the method used to update voxels colour.

TColourUpdate getVoxelColourMethod()

Get the method used to update voxels colour.

virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const

Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object.

virtual bool isEmpty() const

Returns true if the map is empty/no observation has been inserted.

OCTOMAP_CLASS& getOctomap()

Get a reference to the internal octomap object.

Example:

mrpt::maps::COctoMap  map;
octomap::OcTree &om = map.getOctomap<octomap::OcTree>();

virtual std::string asString() const

Returns a short description of the map.

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 void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const

Returns a 3D object representing the map.

See also:

renderingOptions

bool getPointOccupancy(
    const float x,
    const float y,
    const float z,
    double& prob_occupancy
    ) const

Get the occupancy probability [0,1] of a point.

Returns:

false if the point is not mapped, in which case the returned “prob” is undefined.

void insertPointCloud(
    const CPointsMap& ptMap,
    const float sensor_x,
    const float sensor_y,
    const float sensor_z
    )

Update the octomap with a 2D or 3D scan, given directly as a point cloud and the 3D location of the sensor (the origin of the rays) in this map’s frame of reference.

Insertion parameters can be found in insertionOptions.

See also:

The generic observation insertion method CMetricMap::insertObservation()

bool castRay(
    const mrpt::math::TPoint3D& origin,
    const mrpt::math::TPoint3D& direction,
    mrpt::math::TPoint3D& end,
    bool ignoreUnknownCells = false,
    double maxRange = -1.0
    ) const

Performs raycasting in 3d, similar to computeRay().

A ray is cast from origin with a given direction, the first occupied cell is returned (as center coordinate). If the starting coordinate is already occupied in the tree, this coordinate will be returned as a hit.

Parameters:

origin	starting coordinate of ray
direction	A vector pointing in the direction of the raycast. Does not need to be normalized.
end	returns the center of the cell that was hit by the ray, if successful
ignoreUnknownCells	whether unknown cells are ignored. If false (default), the raycast aborts when an unkown cell is hit.
maxRange	Maximum range after which the raycast is aborted (<= 0: no limit, default)

Returns:

whether or not an occupied cell was hit

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

Computes the matching between this and another 2D point map, which includes finding:

The set of points pairs in each map
The mean squared distance between corresponding pairs.

The algorithm is:

For each point in “otherMap”:
- Transform the point according to otherMapPose
- Search with a KD-TREE the closest correspondences in “this” map.
- Add to the set of candidate matchings, if it passes all the thresholds in params.

This method is the most time critical one into ICP-like algorithms.

Parameters:

otherMap	[IN] The other map to compute the matching with.
otherMapPose	[IN] The pose of the other map as seen from “this”.
params	[IN] Parameters for the determination of pairings.
correspondences	[OUT] The detected matchings pairs.
extraResults	[OUT] Other results.

See also:

compute3DMatchingRatio

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

Computes the ratio in [0,1] of correspondences between “this” and the “otherMap” map, whose 6D pose relative to “this” is “otherMapPose” In the case of a multi-metric map, this returns the average between the maps.

This method always return 0 for grid maps.

Parameters:

otherMap	[IN] The other map to compute the matching with.
otherMapPose	[IN] The 6D pose of the other map as seen from “this”.
params	[IN] Matching parameters

Returns:

The matching ratio [0,1]

See also:

determineMatching2D