template class mrpt::maps::CVoxelMapOccupancyBase

Overview

Base class for log-odds sparse voxel map for cells containing occupancy, and possibly other information, for each voxel.

See also:

Use derived classes CVoxelMap, CVoxelMapRGB

#include <mrpt/maps/CVoxelMapOccupancyBase.h>

template <typename voxel_node_t, typename occupancy_t = int8_t>
class CVoxelMapOccupancyBase:
    public mrpt::maps::CVoxelMapBase,
    public mrpt::maps::detail::logoddscell_traits<int8_t>,
    public mrpt::maps::NearestNeighborsCapable,
    public mrpt::config::OptionsCapable
{
public:
    // fields

    TVoxelMap_InsertionOptions insertionOptions;
    TVoxelMap_LikelihoodOptions likelihoodOptions;
    TVoxelMap_RenderingOptions renderingOptions;

    // construction

    CVoxelMapOccupancyBase(
        double resolution = 0.05,
        uint8_t inner_bits = 2,
        uint8_t leaf_bits = 3
        );

    // methods

    virtual void nn_prepare_for_2d_queries() const;
    virtual void nn_prepare_for_3d_queries() const;
    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 bool isEmpty() const;
    virtual void getAsOctoMapVoxels(mrpt::viz::COctoMapVoxels& gl_obj) const;
    void updateVoxel(const double x, const double y, const double z, bool occupied);
    bool getPointOccupancy(const double x, const double y, const double z, double& prob_occupancy) const;

    void insertPointCloudAsRays(
        const mrpt::maps::CPointsMap& pts,
        const mrpt::math::TPoint3D& sensorPt,
        const std::optional<const mrpt::poses::CPose3D>& sensorPose = std::nullopt
        );

    void insertPointCloudAsEndPoints(
        const mrpt::maps::CPointsMap& pts,
        const mrpt::math::TPoint3D& sensorPt,
        const std::optional<const mrpt::poses::CPose3D>& sensorPose = std::nullopt
        );

    mrpt::maps::CSimplePointsMap::ConstPtr getOccupiedVoxels() const;
    mrpt::maps::CSimplePointsMap::Ptr getOccupiedVoxels();
    virtual mrpt::math::TBoundingBoxf boundingBox() const;
    virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName();
    void updateCell_fast_occupied(voxel_node_t* theCell, const occupancy_t logodd_obs, const occupancy_t thres);
    void updateCell_fast_occupied(const Bonxai::CoordT& coord, const occupancy_t logodd_obs, const occupancy_t thres);
    void updateCell_fast_free(voxel_node_t* theCell, const occupancy_t logodd_obs, const occupancy_t thres);
    void updateCell_fast_free(const Bonxai::CoordT& coord, const occupancy_t logodd_obs, const occupancy_t thres);
    static CLogOddsGridMapLUT<occupancy_value_t>& get_logodd_lut();
    static float l2p(const occupancy_value_t l);
    static uint8_t l2p_255(const occupancy_value_t l);
    static occupancy_value_t p2l(const float p);
};

// direct descendants

class CVoxelMap;
class CVoxelMapRGB;

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 CVoxelMapBase<node_t> myself_t;
    typedef node_t voxel_node_t;

    // structs

    struct Impl;

    // fields

    TMapGenericParams genericMapParams;
    static constexpr int8_t CELLTYPE_MIN = -127;
    static constexpr int8_t CELLTYPE_MAX = 127;
    static constexpr int8_t P2LTABLE_SIZE = CELLTYPE_MAX;
    static constexpr std::size_t LOGODDS_LUT_ENTRIES = 1<<8;

    // 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;
    CVoxelMapBase& operator = (const CVoxelMapBase&);
    CVoxelMapBase& operator = (CVoxelMapBase&& o);
    const Bonxai::VoxelGrid<node_t>& grid() const;
    virtual std::string asString() const;
    virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& o) const;
    virtual void getAsOctoMapVoxels(mrpt::viz::COctoMapVoxels& gl_obj) const = 0;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    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);

Fields

TVoxelMap_InsertionOptions insertionOptions

The options used when inserting observations in the map:

Methods

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.

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 bool isEmpty() const

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

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

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

Implementation defined for each children class.

See also:

renderingOptions

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 getPointOccupancy(
    const double x,
    const double y,
    const double 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.

mrpt::maps::CSimplePointsMap::ConstPtr getOccupiedVoxels() const

Returns all occupied voxels as a point cloud.

The shared_ptr is also hold and updated internally, so it is not safe to read it while also updating the voxel map in another thread.

The point cloud is cached, and invalidated upon map updates.

A voxel is considered occupied if its occupancy is larger than likelihoodOptions.occupiedThreshold (Range: [0,1], default: 0.6)

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

This visits all cells to calculate a bounding box, caching the result so subsequent calls are cheap until the voxelmap is changed in some way.

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.

void updateCell_fast_occupied(
    voxel_node_t* theCell,
    const occupancy_t logodd_obs,
    const occupancy_t thres
    )

Performs Bayesian fusion of a new observation of a cell.

This method increases the “occupancy-ness” of a cell, managing possible saturation.

Parameters:

theCell	The cell to modify
logodd_obs	Observation of the cell, in log-odd form as transformed by p2l.
thres	This must be CELLTYPE_MIN+logodd_obs

See also:

updateCell, updateCell_fast_free

void updateCell_fast_occupied(
    const Bonxai::CoordT& coord,
    const occupancy_t logodd_obs,
    const occupancy_t thres
    )

Performs Bayesian fusion of a new observation of a cell.

This method increases the “occupancy-ness” of a cell, managing possible saturation.

Parameters:

coord	Cell indexes.
logodd_obs	Observation of the cell, in log-odd form as transformed by p2l.
thres	This must be CELLTYPE_MIN+logodd_obs

See also:

updateCell, updateCell_fast_free

void updateCell_fast_free(
    voxel_node_t* theCell,
    const occupancy_t logodd_obs,
    const occupancy_t thres
    )

Performs Bayesian fusion of a new observation of a cell.

This method increases the “free-ness” of a cell, managing possible saturation.

Parameters:

logodd_obs	Observation of the cell, in log-odd form as transformed by p2l.
thres	This must be CELLTYPE_MAX-logodd_obs

See also:

updateCell_fast_occupied

void updateCell_fast_free(
    const Bonxai::CoordT& coord,
    const occupancy_t logodd_obs,
    const occupancy_t thres
    )

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

This method increases the “free-ness” of a cell, managing possible saturation.

Parameters:

coord	Cell indexes.
logodd_obs	Observation of the cell, in log-odd form as transformed by p2l.
thres	This must be CELLTYPE_MAX-logodd_obs

See also:

updateCell_fast_occupied

static CLogOddsGridMapLUT<occupancy_value_t>& get_logodd_lut()

Lookup tables for log-odds.

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

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

static occupancy_value_t p2l(const float p)

Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the valid range of voxel_node_t.