class mrpt::maps::COccupancyGridMap3D

Python API: mrpt.maps.COccupancyGridMap3D

Overview

A 3D occupancy grid map with a regular, even distribution of voxels.

This is a faster alternative to COctoMap, but use with caution with limited map extensions, since it could easily exaust available memory.

Each voxel follows a Bernoulli probability distribution: a value of 0 means certainly occupied, 1 means a certainly empty voxel. Initially 0.5 means uncertainty.

An alternative, sparse representation of a 3D map is provided via mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB

#include <mrpt/maps/COccupancyGridMap3D.h>

class COccupancyGridMap3D:
    public mrpt::maps::CMetricMap,
    public mrpt::maps::CLogOddsGridMap3D,
    public mrpt::maps::NearestNeighborsCapable,
    public mrpt::config::OptionsCapable
{
public:
    // typedefs

    typedef std::shared_ptr<mrpt::maps ::COccupancyGridMap3D> Ptr;
    typedef std::shared_ptr<const mrpt::maps ::COccupancyGridMap3D> ConstPtr;
    typedef std::unique_ptr<mrpt::maps ::COccupancyGridMap3D> UniquePtr;
    typedef std::unique_ptr<const mrpt::maps ::COccupancyGridMap3D> ConstUniquePtr;
    typedef OccGridCellTraits::cellType voxelType;
    typedef OccGridCellTraits::cellTypeUnsigned voxelTypeUnsigned;

    // enums

    enum TLikelihoodMethod;

    // structs

    struct TMapDefinition;
    struct TMapDefinitionBase;
    struct TRenderingOptions;

    // classes

    class TInsertionOptions;
    class TLikelihoodOptions;

    // fields

    static constexpr const char* className = "mrpt::maps" "::" "COccupancyGridMap3D";
    static const size_t m_private_map_register_id =   mrpt::maps::internal::TMetricMapTypesRegistry::Instance().doRegister("mrpt::maps::COccupancyGridMap3D" ,& mrpt::maps::COccupancyGridMap3D ::MapDefinition,& mrpt::maps::COccupancyGridMap3D ::internal_CreateFromMapDefinition);
    TInsertionOptions insertionOptions;
    TLikelihoodOptions likelihoodOptions;
    TRenderingOptions renderingOptions;

    // construction

    COccupancyGridMap3D(
        const mrpt::math::TPoint3D& corner_min = {-5.0f, -5.0f, -5.0f},
        const mrpt::math::TPoint3D& corner_max = {5.0f, 5.0f, 5.0f},
        float resolution = 0.25f
        );

    // methods

    static constexpr auto getClassName();
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    static std::shared_ptr<CObject> CreateObject();

    template <typename... Args>
    static Ptr Create(Args&&... args);

    template <typename Alloc, typename... Args>
    static Ptr CreateAlloc(
        const Alloc& alloc,
        Args&&... args
        );

    template <typename... Args>
    static UniquePtr CreateUnique(Args&&... args);

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual mrpt::rtti::CObject* clone() const;
    static std::shared_ptr<mrpt::maps::TMetricMapInitializer> MapDefinition();
    static std::shared_ptr<COccupancyGridMap3D> CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer& def);
    static std::shared_ptr<mrpt::maps::CMetricMap> internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer& def);
    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;

    static float l2p(const voxelType l);
    static uint8_t l2p_255(const voxelType l);
    static voxelType p2l(const float p);
    void fill(float default_value = 0.5f);

    void setSize(
        const mrpt::math::TPoint3D& corner_min,
        const mrpt::math::TPoint3D& corner_max,
        double resolution,
        float default_value = 0.5f
        );

    void resizeGrid(
        const mrpt::math::TPoint3D& corner_min,
        const mrpt::math::TPoint3D& corner_max,
        float new_voxels_default_value = 0.5f
        );

    void updateCell(int cx_idx, int cy_idx, int cz_idx, float v);
    void setCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz, float value);
    float getCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz) const;
    void setFreenessByPos(float x, float y, float z, float value);
    float getFreenessByPos(float x, float y, float z) const;
    void insertRay(const mrpt::math::TPoint3D& sensor, const mrpt::math::TPoint3D& end, bool endIsOccupied = true);

    void insertPointCloud(
        const mrpt::math::TPoint3D& sensorCenter,
        const mrpt::maps::CPointsMap& pts,
        const float maxValidRange = std::numeric_limits<float>::max(),
        const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt
        );

    void getAsOctoMapVoxels(mrpt::viz::COctoMapVoxels& gl_obj) const;
    virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& outObj) const;
    virtual mrpt::math::TBoundingBoxf boundingBox() const;
    virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName();
    virtual bool isEmpty() 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;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    virtual std::string asString() const;
};

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    typedef std::unique_ptr<CObject> UniquePtr;
    typedef std::unique_ptr<const CObject> ConstUniquePtr;
    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;
    typedef std::shared_ptr<CMetricMap> Ptr;
    typedef std::shared_ptr<const CMetricMap> ConstPtr;
    typedef TCELL cell_t;
    typedef detail::logoddscell_traits<TCELL> traits_t;
    typedef mrpt::containers::CDynamicGrid3D<TCELL> grid_t;

    // fields

    TMapGenericParams genericMapParams;
    grid_t m_grid;

    // methods

    mrpt::rtti::CObject::Ptr duplicateGetSmartPtr() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual CObject* clone() const = 0;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual std::string asString() const = 0;
    Visualizable& operator = (const Visualizable&);
    Visualizable& operator = (Visualizable&&);
    virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& o) const = 0;
    std::shared_ptr<mrpt::viz::CSetOfObjects> getVisualization() const;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    void clear();
    virtual bool isEmpty() const = 0;
    virtual auto boundingBox() const;
    void loadFromSimpleMap(const mrpt::maps::CSimpleMap& Map);
    bool insertObservation(const mrpt::obs::CObservation& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt);
    bool insertObservationPtr(const mrpt::obs::CObservation::Ptr& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt);
    double computeObservationLikelihood(const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const;
    virtual bool canComputeObservationLikelihood(const mrpt::obs::CObservation& obs) const;
    double computeObservationsLikelihood(const mrpt::obs::CSensoryFrame& sf, const mrpt::poses::CPose3D& takenFrom);
    bool canComputeObservationsLikelihood(const mrpt::obs::CSensoryFrame& sf) 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 void determineMatching3D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose3D& 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 = 0;
    virtual void auxParticleFilterCleanUp();
    virtual float squareDistanceToClosestCorrespondence(float x0, float y0) const;
    void updateCell_fast_occupied(cell_t* theCell, const cell_t logodd_obs, const cell_t thres);

    void updateCell_fast_occupied(
        const unsigned x,
        const unsigned y,
        const unsigned z,
        const cell_t logodd_obs,
        const cell_t thres
        );

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

    void updateCell_fast_free(
        const unsigned x,
        const unsigned y,
        const unsigned z,
        const cell_t logodd_obs,
        const cell_t thres
        );

    virtual bool nn_has_indices_or_ids() const = 0;
    virtual void nn_prepare_for_2d_queries() const;
    virtual void nn_prepare_for_3d_queries() const;
    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,
        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,
        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,
        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,
        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;

    NearestNeighborsCapable& operator = (const NearestNeighborsCapable&);
    NearestNeighborsCapable& operator = (NearestNeighborsCapable&&);
    OptionsCapable& operator = (const OptionsCapable&);
    OptionsCapable& operator = (OptionsCapable&&);
    virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName() = 0;
    virtual bool trySetCreationOptions(] const mrpt::config::CConfigFileBase& cfg, ] const std::string& section);

Typedefs

typedef std::shared_ptr<mrpt::maps ::COccupancyGridMap3D> Ptr

A type for the associated smart pointer.

typedef OccGridCellTraits::cellType voxelType

The type of the map voxels:

Fields

static const size_t m_private_map_register_id =   mrpt::maps::internal::TMetricMapTypesRegistry::Instance().doRegister("mrpt::maps::COccupancyGridMap3D" ,& mrpt::maps::COccupancyGridMap3D ::MapDefinition,& mrpt::maps::COccupancyGridMap3D ::internal_CreateFromMapDefinition)

ID used to initialize class registration (just ignore it)

TInsertionOptions insertionOptions

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

See also:

CObservation::insertIntoGridMap

Construction

COccupancyGridMap3D(
    const mrpt::math::TPoint3D& corner_min = {-5.0f, -5.0f, -5.0f},
    const mrpt::math::TPoint3D& corner_max = {5.0f, 5.0f, 5.0f},
    float resolution = 0.25f
    )

Constructor.

Methods

virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const

Returns information about the class of an object in runtime.

virtual mrpt::rtti::CObject* clone() const

Returns a deep copy (clone) of the object, indepently of its class.

static std::shared_ptr<mrpt::maps::TMetricMapInitializer> MapDefinition()

Returns default map definition initializer.

See * mrpt::maps::TMetricMapInitializer

static std::shared_ptr<COccupancyGridMap3D> CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer& def)

Constructor from a map definition structure: initializes the map and * its parameters accordingly.

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.

static float l2p(const voxelType 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 voxelType l)

Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l))

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

void fill(float default_value = 0.5f)

Fills all the voxels with a default value.

void setSize(
    const mrpt::math::TPoint3D& corner_min,
    const mrpt::math::TPoint3D& corner_max,
    double resolution,
    float default_value = 0.5f
    )

Change the size of gridmap, erasing all its previous contents.

Parameters:

resolution	The new size of voxels.
default_value	The value of voxels, typically 0.5.

See also:

ResizeGrid

void resizeGrid(
    const mrpt::math::TPoint3D& corner_min,
    const mrpt::math::TPoint3D& corner_max,
    float new_voxels_default_value = 0.5f
    )

Change the size of gridmap, maintaining previous contents.

Parameters:

new_voxels_default_value

Value of new voxels, typically 0.5

See also:

setSize()

void updateCell(int cx_idx, int cy_idx, int cz_idx, float v)

Performs the Bayesian fusion of a new observation of a cell.

See also:

updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free

void setCellFreeness(
    unsigned int cx,
    unsigned int cy,
    unsigned int cz,
    float value
    )

Change the contents [0,1] (0:occupied, 1:free) of a voxel, given its index.

float getCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz) const

Read the real valued [0,1] (0:occupied, 1:free) contents of a voxel, given its index.

void setFreenessByPos(float x, float y, float z, float value)

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

float getFreenessByPos(float x, float y, float z) const

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

void insertRay(const mrpt::math::TPoint3D& sensor, const mrpt::math::TPoint3D& end, bool endIsOccupied = true)

Increases the freeness of a ray segment, and the occupancy of the voxel at its end point (unless endIsOccupied=false).

Normally, users would prefer the higher-level method CMetricMap::insertObservation()

void insertPointCloud(
    const mrpt::math::TPoint3D& sensorCenter,
    const mrpt::maps::CPointsMap& pts,
    const float maxValidRange = std::numeric_limits<float>::max(),
    const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt
    )

Calls insertRay() for each point in the point cloud, using as sensor central point (the origin of all rays), the given sensorCenter.

Parameters:

maxValidRange

If a point has larger distance from sensorCenter than maxValidRange, it will be considered a non-echo, and NO occupied voxel will be created at the end of the segment.

See also:

insertionOptions parameters are observed in this method.

void getAsOctoMapVoxels(mrpt::viz::COctoMapVoxels& gl_obj) const

See also:

renderingOptions

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

Returns a 3D object representing the map.

See also:

renderingOptions

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.

virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName()

Maps an options-group name (e.g.

“insertionOptions”) to a pointer to the corresponding, live CLoadableOptions member. Pointers remain valid as long as this is alive, and point directly to the actual option members (no copies), so writes through them (e.g. via loadFromConfigFile()) take effect immediately.

Implementations should list every CLoadableOptions member they define, INCLUDING “creationOptions” if present (so it can be discovered/exported generically); however, callers must use trySetCreationOptions(), not a direct write through this pointer, to safely modify creation options.

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.

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

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

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.