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:
    // 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

    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,
        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 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,
        const 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:
    // 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&&);

Typedefs

typedef uint64_t KeyFrameID

key-frame ID, index in the keyframes_ map.

Construction

KeyframePointCloudMap()

Constructor / default ctor.

Methods

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.

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