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;
    uint64_t covarianceComputations() const;
    std::size_t cachedCovarianceCount() 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;

    virtual std::size_t point_count() const;
    std::string asString() const;
    bool isEmpty() const;
    void getVisualizationInto(mrpt::viz::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
        );

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

    virtual void nn_search_cov2cov(
        const NearestPointWithCovCapable& localMap,
        const mrpt::poses::CPose3D& localMapPose,
        const MatchingDistanceProfile& matchingDistance,
        MatchedPointWithCovList& outPairings
        ) const;
};

Inherited Members

public:
    // methods

    NearestPointWithCovCapable& operator = (const NearestPointWithCovCapable&);
    NearestPointWithCovCapable& operator = (NearestPointWithCovCapable&&);
    virtual std::size_t point_count() const = 0;
    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.

uint64_t covarianceComputations() const

Number of per-point covariance estimations run so far, i.e. cache misses plus the points whose neighborhood never qualified for caching. For diagnostics and benchmarks.

See also:

TCreationOptions::min_neighbors_to_cache_cov

std::size_t cachedCovarianceCount() const

Number of points whose covariance is currently held in the cache.

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.

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

Implements search for pairings between me (“global”) and another (“local”) map outPairings may not be empty, so implementations must add values here, never clear it.

This flat-threshold form remains the pure virtual one on purpose: map classes written against mp2p_icp releases that predate MatchingDistanceProfile keep overriding it verbatim, so they still compile and behave identically. New implementations should override the MatchingDistanceProfile form below and implement this one as a one-line forwarder.

Declaring only one of the two overloads in a derived class hides the other for calls made on that derived type (ordinary C++ name hiding). This is harmless for the matchers, which always dispatch through a NearestPointWithCovCapable reference, but a derived class that wants both visible on its own type should add using NearestPointWithCovCapable::nn_search_cov2cov;.

virtual void nn_search_cov2cov(
    const NearestPointWithCovCapable& localMap,
    const mrpt::poses::CPose3D& localMapPose,
    const MatchingDistanceProfile& matchingDistance,
    MatchedPointWithCovList& outPairings
    ) const

As above, but with the full acceptance criteria: an optionally range-adaptive matching distance.

Not pure: the default implementation forwards to the flat overload above, so an implementation predating this API keeps working. Since such an implementation cannot honor anything beyond a flat distance, the default throws rather than silently ignoring a profile it cannot apply.