class mola::KeyframePointCloudMap

Overview

An efficient storage class for large point clouds built as keyframes, each having an associated local cloud.

The user of the class is responsible for processing raw observations into mrpt::obs::CObservationPointCloud observations, the only ones allowed as input to the insert*() methods, with points already transformed from the sensor frame to the vehicle (base_link) frame. This can be easily done with mp2p_icp::Generator, plus an optional filtering pipeline.

Each key-frame is responsible of keeping its own KD-tree for NN searches and keeping up-to-date covariances for each point local vicinity.

#include <KeyframePointCloudMap.h>

class KeyframePointCloudMap:
    public mrpt::maps::CMetricMap,
    public mrpt::maps::NearestNeighborsCapable,
    public mp2p_icp::IcpPrepareCapable,
    public mp2p_icp::NearestPointWithCovCapable,
    public mp2p_icp::MetricMapMergeCapable,
    public mola::KeyframeMapCapable
{
public:
    // typedefs

    typedef uint64_t KeyFrameID;

    // structs

    struct CachedData;
    struct TCreationOptions;
    struct TInsertionOptions;
    struct TLikelihoodOptions;
    struct TRenderOptions;

    // classes

    class KeyFrame;

    // fields

    TInsertionOptions insertionOptions;
    TLikelihoodOptions likelihoodOptions;
    TRenderOptions renderOptions;
    TCreationOptions creationOptions;
    mola::KeyframePointCloudMap::TCreationOptions creationOptions;
    mola::KeyframePointCloudMap::TInsertionOptions insertionOpts;
    mola::KeyframePointCloudMap::TLikelihoodOptions likelihoodOpts;
    mola::KeyframePointCloudMap::TRenderOptions renderOpts;

    // construction

    KeyframePointCloudMap();
    KeyframePointCloudMap(const KeyframePointCloudMap&);
    KeyframePointCloudMap(KeyframePointCloudMap&&);

    // methods

    KeyFrameID nextFreeKeyFrameID_public() const;
    std::map<KeyFrameID, mrpt::poses::CPose3D> cloneKFPoses() const;
    virtual void setKeyframePose(KeyFrameID id, const mrpt::poses::CPose3D& new_pose);
    virtual std::map<KeyFrameID, mrpt::poses::CPose3D> keyframePoses() const;
    virtual std::optional<KeyFrameID> oldestActiveKeyframeID() const;
    virtual void applyPivotTransform(KeyFrameID pivot_id, const mrpt::poses::CPose3D& delta_at_pivot);
    std::optional<KeyFrameID> lastInsertedKeyFrameID() const;
    std::vector<KeyFrameID> drainEvictedKeyFrameIDs();
    mrpt::math::TBoundingBoxf boundingBox() 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,
        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,
        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,
        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,
        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,
        size_t N,
        std::vector<mrpt::math::TPoint3Df>& results,
        std::vector<float>& out_dists_sqr,
        std::vector<uint64_t>& resultIndicesOrIDs
        ) const;

    virtual void icp_get_prepared_as_global(
        const mrpt::poses::CPose3D& icp_ref_point,
        const std::optional<mrpt::math::TBoundingBoxf>& local_map_roi = std::nullopt
        ) const;

    virtual void icp_cleanup() const;

    void merge_with(
        const MetricMapMergeCapable& source,
        const std::optional<mrpt::poses::CPose3D>& otherRelativePose = std::nullopt
        );

    void transform_map_left_multiply(const mrpt::poses::CPose3D& b);

    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;
    void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const;
    bool isEmpty() const;
    void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    const mrpt::maps::CSimplePointsMap* getAsSimplePointsMap() const;
    KeyframePointCloudMap& operator = (const KeyframePointCloudMap&);
    KeyframePointCloudMap& operator = (KeyframePointCloudMap&&);

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

    virtual void merge_with(
        const MetricMapMergeCapable& source,
        const std::optional<mrpt::poses::CPose3D>& otherRelativePose = std::nullopt
        ) = 0;

    virtual void transform_map_left_multiply(] const mrpt::poses::CPose3D& b);
};

Inherited Members

public:
    // typedefs

    typedef uint64_t KeyFrameID;

    // methods

    IcpPrepareCapable& operator = (const IcpPrepareCapable&);
    IcpPrepareCapable& operator = (IcpPrepareCapable&&);

    virtual void icp_get_prepared_as_global(
        const mrpt::poses::CPose3D& icp_ref_point,
        const std::optional<mrpt::math::TBoundingBoxf>& local_map_roi = std::nullopt
        ) const = 0;

    virtual void icp_cleanup() const;
    NearestPointWithCovCapable& operator = (const NearestPointWithCovCapable&);
    NearestPointWithCovCapable& operator = (NearestPointWithCovCapable&&);
    virtual std::size_t point_count() const = 0;
    MetricMapMergeCapable& operator = (const MetricMapMergeCapable&);
    MetricMapMergeCapable& operator = (MetricMapMergeCapable&&);
    KeyframeMapCapable& operator = (const KeyframeMapCapable&);
    KeyframeMapCapable& operator = (KeyframeMapCapable&&);
    virtual std::map<KeyFrameID, mrpt::poses::CPose3D> keyframePoses() const = 0;
    virtual std::optional<KeyFrameID> oldestActiveKeyframeID() const = 0;
    virtual void setKeyframePose(KeyFrameID id, const mrpt::poses::CPose3D& new_pose) = 0;
    virtual void applyPivotTransform(KeyFrameID pivot_id, const mrpt::poses::CPose3D& delta_at_pivot) = 0;

Typedefs

typedef uint64_t KeyFrameID

key-frame ID, index in the keyframes_ map.

Construction

KeyframePointCloudMap()

Constructor / default ctor.

Methods

KeyFrameID nextFreeKeyFrameID_public() const

The KF id that will be assigned to the next inserted key-frame.

std::map<KeyFrameID, mrpt::poses::CPose3D> cloneKFPoses() const

Snapshot of all key-frame poses, keyed by KF id. Cheap. Thread-safe.

virtual void setKeyframePose(KeyFrameID id, const mrpt::poses::CPose3D& new_pose)

Overwrites the pose of one KF in the map, invalidating its caches. No-op if id is not present. Thread-safe.

virtual std::map<KeyFrameID, mrpt::poses::CPose3D> keyframePoses() const

Returns a snapshot of all currently-active keyframe poses, keyed by KF id. The returned map is a deep copy and is safe to hold across subsequent map mutations.

virtual std::optional<KeyFrameID> oldestActiveKeyframeID() const

Returns the smallest currently-active KF id, or nullopt if the map is empty. Used as the natural pivot for online tilt correction (oldest active KF stays put).

virtual void applyPivotTransform(
    KeyFrameID pivot_id,
    const mrpt::poses::CPose3D& delta_at_pivot
    )

Applies a pivot-based rigid SE(3) transform to all currently-active KFs.

Implementations are encouraged to delegate to their existing transform_map_left_multiply() after computing: T_correct = T_pivot + delta_at_pivot + (-T_pivot) No-op if pivot_id is not present in the active set.

std::optional<KeyFrameID> lastInsertedKeyFrameID() const

The id of the last key-frame successfully inserted, or nullopt if none has been inserted since the map was created/cleared.

std::vector<KeyFrameID> drainEvictedKeyFrameIDs()

Returns and clears the list of KF ids evicted since the last call (or since map construction). Used by callers that maintain per-KF auxiliary state and need to drop it on eviction.

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.

virtual void icp_get_prepared_as_global(
    const mrpt::poses::CPose3D& icp_ref_point,
    const std::optional<mrpt::math::TBoundingBoxf>& local_map_roi = std::nullopt
    ) const

Prepare the map for ICP with a given point as reference.

virtual void icp_cleanup() const

Optionally, clean up after ICP is done.

void transform_map_left_multiply(const mrpt::poses::CPose3D& b)

Transform all KF poses left-multiplying with this transform. Thread safe.

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 entire map. Not efficient at all. Only for MOLA->ROS2 bridge.

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.

virtual void merge_with(
    const MetricMapMergeCapable& source,
    const std::optional<mrpt::poses::CPose3D>& otherRelativePose = std::nullopt
    ) = 0

Merge

virtual void transform_map_left_multiply(] const mrpt::poses::CPose3D& b)

Change the map such as each entity \(p_i\) becomes \(p'_i = b \oplus p_i\) (pose compounding operator).