class mola::SparseVoxelPointCloud

Overview

SparseVoxelPointCloud : a pointcloud stored as a sparse-dense set of cubic voxel maps. Efficient for storing point clouds, decimating them, and running nearest nearest-neighbor search.

#include <SparseVoxelPointCloud.h>

class SparseVoxelPointCloud:
    public mrpt::maps::CMetricMap,
    public mrpt::maps::NearestNeighborsCapable
{
public:
    // typedefs

    typedef index3d_t<int32_t> global_index3d_t;
    typedef index3d_t<int32_t> outer_index3d_t;
    typedef index3d_t<uint32_t> inner_index3d_t;
    typedef uint32_t inner_plain_index_t;
    typedef uint64_t global_plain_index_t;
    typedef std::map<outer_index3d_t, InnerGrid, index3d_hash<int32_t>> grids_map_t;

    // structs

    struct CachedData;
    struct InnerGrid;
    struct TInsertionOptions;
    struct TLikelihoodOptions;
    struct TRenderOptions;
    struct VoxelData;

    // fields

    static constexpr static std::size_t HARDLIMIT_MAX_POINTS_PER_VOXEL = 16;
    static constexpr static uint32_t INNER_GRID_BIT_COUNT = 5;
    static constexpr static std::size_t GLOBAL_ID_SUBVOXEL_BITCOUNT = 4;
    static constexpr static uint32_t INNER_GRID_SIDE = 1<<INNER_GRID_BIT_COUNT;
    static constexpr static uint32_t INNER_COORDS_MASK = INNER_GRID_SIDE - 1;
    static constexpr static uint32_t OUTER_COORDS_MASK = ~INNER_COORDS_MASK;
    TInsertionOptions insertionOptions;
    TLikelihoodOptions likelihoodOptions;
    TRenderOptions renderOptions;
    float voxel_size = 0.20f;
    mola::SparseVoxelPointCloud::TInsertionOptions insertionOpts;
    mola::SparseVoxelPointCloud::TLikelihoodOptions likelihoodOpts;
    mola::SparseVoxelPointCloud::TRenderOptions renderOpts;

    // construction

    SparseVoxelPointCloud(float voxel_size = 0.20f);

    // methods

    static outer_index3d_t g2o(const global_index3d_t& g);
    static inner_index3d_t g2i(const global_index3d_t& g);
    static inner_index3d_t plain2i(const inner_plain_index_t& p);

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

    global_index3d_t coordToGlobalIdx(const mrpt::math::TPoint3Df& pt) const;
    mrpt::math::TPoint3Df globalIdxToCoord(const global_index3d_t idx) const;
    void setVoxelProperties(float voxel_size);
    std::tuple<VoxelData*, InnerGrid*> voxelByGlobalIdxs(const global_index3d_t& idx, bool createIfNew);

    std::tuple<const VoxelData*, const InnerGrid*> voxelByGlobalIdxs(
        const global_index3d_t& idx,
        bool createIfNew
        ) const;

    std::tuple<VoxelData&, InnerGrid&> voxelByCoords(const mrpt::math::TPoint3Df& pt);
    std::tuple<const VoxelData&, const InnerGrid&> voxelByCoords(const mrpt::math::TPoint3Df& pt) const;
    void insertPoint(const mrpt::math::TPoint3Df& pt);
    const grids_map_t& grids() 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 outer_index3d_t&, const inner_plain_index_t, const VoxelData&, const InnerGrid&)>& f) const;
    void visitAllGrids(const std::function<void(const outer_index3d_t&, const InnerGrid&)>& 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;

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

Typedefs

typedef uint64_t global_plain_index_t

collapsed plain unique ID for global indices

Fields

static constexpr static std::size_t HARDLIMIT_MAX_POINTS_PER_VOXEL = 16

Size of the std::array for the small-size optimization container in each voxel, defining the maximum number of points that can be stored without heap allocation.

Construction

SparseVoxelPointCloud(float voxel_size = 0.20f)

Constructor / default ctor.

Parameters:

voxel_size

Voxel size [meters] for decimation purposes.

Methods

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

returns the coordinate of the voxel center

void setVoxelProperties(float voxel_size)

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

std::tuple<VoxelData*, InnerGrid*> 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”

std::tuple<VoxelData&, InnerGrid&> voxelByCoords(const mrpt::math::TPoint3Df& pt)

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

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

Insert one point into the dual voxel map

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