class mola::IncrementalPointCloud

Overview

A single-global-frame, sliding-window point map for LiDAR (inertial) odometry, backed by one incremental, self-balancing nanoflann k-d tree instead of a static tree rebuilt on every scan.

#include <IncrementalPointCloud.h>

class IncrementalPointCloud:
    public mrpt::maps::CGenericPointsMap,
    public mp2p_icp::NearestPointWithCovCapable,
    public mola::OptionsCapable
{
public:
    // structs

    struct TCreationOptions;

    // fields

    TCreationOptions creationOptions;
    mola::IncrementalPointCloud::TCreationOptions creationOpts;
    mrpt::maps::CPointsMap::TInsertionOptions insertionOpts;
    mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts;
    mrpt::maps::CPointsMap::TRenderOptions renderOpts;

    // construction

    IncrementalPointCloud();
    IncrementalPointCloud(const IncrementalPointCloud& o);

    // methods

    std::size_t livePointCount() const;
    std::size_t recyclableSlotCount() const;
    void compact();
    void keepOnlyPointsNear(const mrpt::math::TPoint3Df& center, double half_side);
    mrpt::maps::CGenericPointsMap::Ptr liveCompactedCopy() const;
    mrpt::math::TBoundingBoxf boundingBox() const;
    void reserve(std::size_t newLength);
    void resize(std::size_t newLength);
    void setSize(std::size_t newLength);
    void changeCoordinatesReference(const mrpt::poses::CPose2D& b);
    void changeCoordinatesReference(const mrpt::poses::CPose3D& b);

    void changeCoordinatesReference(
        const mrpt::maps::CPointsMap& other,
        const mrpt::poses::CPose3D& b
        );

    bool nn_has_indices_or_ids() const;
    size_t nn_index_count() const;
    void nn_prepare_for_3d_queries() 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;

    void nn_search_cov2cov(
        const NearestPointWithCovCapable& localMap,
        const mrpt::poses::CPose3D& localMapPose,
        float max_search_distance,
        mp2p_icp::MatchedPointWithCovList& outPairings
        ) const;

    std::size_t point_count() const;
    std::string asString() const;
    bool isEmpty() const;
    void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const;
    void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    const mrpt::maps::CSimplePointsMap* getAsSimplePointsMap() const;
    IncrementalPointCloud& operator = (const IncrementalPointCloud& o);
    virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName();

    bool trySetCreationOptions(
        const mrpt::config::CConfigFileBase& cfg,
        const std::string& section
        );
};

Inherited Members

public:
    // methods

    OptionsCapable& operator = (const OptionsCapable&);
    OptionsCapable& operator = (OptionsCapable&&);
    virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName() = 0;

Methods

std::size_t livePointCount() const

Number of points actually in the map, i.e. excluding the tombstoned storage slots still counted by the inherited size().

std::size_t recyclableSlotCount() const

Number of storage slots ready to be reused by the next insertion.

void compact()

Physically drops the tombstoned storage slots (so size() becomes livePointCount()) and rebuilds the k-d tree with the current creationOptions. Point indices previously returned by the nn_* methods are invalidated.

Call it after changing any structural option (async_rebuild, alpha_*, reserve_points) on an already-populated map; trySetCreationOptions() does it for you.

void keepOnlyPointsNear(const mrpt::math::TPoint3Df& center, double half_side)

Removes every point outside the axis-aligned cube of half side half_side centered at center. This is what insertions do automatically when TCreationOptions::remove_points_farther_than is set.

mrpt::maps::CGenericPointsMap::Ptr liveCompactedCopy() const

A copy holding only the live points, in ascending slot order, with all per-point fields preserved.

mrpt::math::TBoundingBoxf boundingBox() const

O(1): the AABB maintained by the k-d tree. It is a conservative superset of the live points, since not-yet-reclaimed points still contribute.

void nn_search_cov2cov(
    const NearestPointWithCovCapable& localMap,
    const mrpt::poses::CPose3D& localMapPose,
    float max_search_distance,
    mp2p_icp::MatchedPointWithCovList& outPairings
    ) const

Pairs each point of localMap (which must also be an IncrementalPointCloud) with its nearest point in this map, weighting the pair by the GICP matrix (COV_global + R*COV_local*R^T)^-1.

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 map’s 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.