class mola::NDT

Overview

3D NDT maps: Voxels with points approximated by Gaussians. Implementation of [1].

This implements two nearest-neighbor virtual APIs:

• mrpt::maps::NearestNeighborsCapable: It will return pairings of local points against global (this map) points in voxels that do not conform planes.

• mp2p_icp::NearestPlaneCapable : It will return points-to-plane pairings, if the local point falls within a voxel with points conforming a plane.

#include <NDT.h>

class NDT:
    public mrpt::maps::CMetricMap,
    public mrpt::maps::NearestNeighborsCapable,
    public mp2p_icp::NearestPlaneCapable
{
public:
    // typedefs

    typedef uint64_t global_plain_index_t;
    typedef tsl::robin_map<global_index3d_t, VoxelData, index3d_hash<int32_t>> grids_map_t;
    typedef index3d_t<int32_t> global_index3d_t;

    // structs

    struct CachedData;
    struct TInsertionOptions;
    struct TLikelihoodOptions;
    struct TRenderOptions;
    struct VoxelData;
    struct point_vector_t;

    // fields

    static constexpr static size_t MAX_POINTS_PER_VOXEL = 16;
    static constexpr static std::size_t GLOBAL_ID_SUBVOXEL_BITCOUNT = 5;
    TInsertionOptions insertionOptions;
    TLikelihoodOptions likelihoodOptions;
    TRenderOptions renderOptions;
    float voxel_size = 1.00f;
    mola::NDT::TInsertionOptions insertionOpts;
    mola::NDT::TLikelihoodOptions likelihoodOpts;
    mola::NDT::TRenderOptions renderOpts;

    // construction

    NDT(float voxel_size = 5.00f);

    // methods

    global_index3d_t coordToGlobalIdx(const mrpt::math::TPoint3Df& pt) const;
    mrpt::math::TPoint3Df globalIdxToCoord(const global_index3d_t idx) const;

    static global_plain_index_t g2plain(
        const global_index3d_t& g,
        int subVoxelIndex = 0
        );

    bool ndt_is_plane(const mp2p_icp::PointCloudEigen& ndt) const;
    void setVoxelProperties(float voxel_size);
    VoxelData* voxelByGlobalIdxs(const global_index3d_t& idx, bool createIfNew);
    const VoxelData* voxelByGlobalIdxs(const global_index3d_t& idx) const;
    VoxelData* voxelByCoords(const mrpt::math::TPoint3Df& pt, bool createIfNew);
    const VoxelData* voxelByCoords(const mrpt::math::TPoint3Df& pt) const;
    void insertPoint(const mrpt::math::TPoint3Df& pt, const mrpt::math::TPoint3Df& sensorPose);
    const grids_map_t& voxels() const;
    mrpt::math::TBoundingBoxf boundingBox() const;
    void visitAllPoints(const std::function<void(const mrpt::math::TPoint3Df&)>& f) const;
    void visitAllVoxels(const std::function<void(const global_index3d_t&, const VoxelData&)>& f) const;
    bool saveToTextFile(const std::string& file) const;
    bool nn_has_indices_or_ids() const;
    size_t nn_index_count() const;

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

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

    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;

    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;

    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;

    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;

    template <size_t MAX_KNN>
    void nn_multiple_search_impl(
        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 NearestPlaneResult nn_search_pt2pl(
        const mrpt::math::TPoint3Df& point,
        const float max_search_distance
        ) const;

    std::string asString() const;
    void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const;
    bool isEmpty() const;
    void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    const mrpt::maps::CSimplePointsMap* getAsSimplePointsMap() const;
};

Inherited Members

public:
    // structs

    struct NearestPlaneResult;

    // methods

    virtual NearestPlaneResult nn_search_pt2pl(const mrpt::math::TPoint3Df& point, const float max_search_distance) const = 0;

Typedefs

typedef uint64_t global_plain_index_t

collapsed plain unique ID for global indices

Construction

NDT(float voxel_size = 5.00f)

Constructor / default ctor.

Parameters:

voxel_size

Voxel size [meters]

Methods

mrpt::math::TPoint3Df globalIdxToCoord(const global_index3d_t idx) const

returns the coordinate of the voxel “bottom” corner

bool ndt_is_plane(const mp2p_icp::PointCloudEigen& ndt) const

Returns true if the fitted Gaussian fulfills the requirements of being planar enough, according to the insertionOptions parameters

void setVoxelProperties(float voxel_size)

Reset the main voxel parameters, and clears all current map contents

VoxelData* voxelByGlobalIdxs(const global_index3d_t& idx, bool createIfNew)

returns the voxeldata by global index coordinates, creating it or not if not found depending on createIfNew. Returns nullptr if not found and createIfNew is false

Function defined in the header file so compilers can optimize for literals “createIfNew”

VoxelData* voxelByCoords(const mrpt::math::TPoint3Df& pt, bool createIfNew)

Get a voxeldata by (x,y,z) coordinates, creating the voxel if needed.

void insertPoint(
    const mrpt::math::TPoint3Df& pt,
    const mrpt::math::TPoint3Df& sensorPose
    )

Insert one point into the map. The sensor pose is used to estimate the outward surface normal

mrpt::math::TBoundingBoxf boundingBox() const

Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points. Results are cached unless the map is somehow modified to avoid repeated calculations.

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

Save to a text file. Each line contains “X Y Z” point coordinates. Returns false if any error occured, true elsewere.

std::string asString() const

Returns a short description of the map.

bool isEmpty() const

Returns true if the map is empty

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

const mrpt::maps::CSimplePointsMap* getAsSimplePointsMap() const

Returns a cached point cloud view of the hash map. Not efficient at all. Only for MOLA->ROS2 bridge.