class mola::SearchablePoseList

Overview

Data structure to search for nearby SE(3) poses.

It uses a KD-tree for the search.

Optionally, each inserted pose can be tagged with an external ID (e.g. a keyframe ID) so that callers can later update its stored pose in place via setPoseById(). This is used by the online gravity-rebake feature to keep distance-checkers in sync with per-KF pose corrections without rebuilding from scratch.

#include <SearchablePoseList.h>

class SearchablePoseList
{
public:
    // typedefs

    typedef uint64_t KFID;

    // structs

    struct NearbyPose;

    // construction

    SearchablePoseList();
    SearchablePoseList(bool measure_from_last_kf_only);

    // methods

    bool empty() const;
    size_t size() const;
    void insert(const mrpt::poses::CPose3D& p);
    void insert(const mrpt::poses::CPose3D& p, KFID id);
    void setPoseById(KFID id, const mrpt::poses::CPose3D& new_pose);
    void transform_left_multiply(const mrpt::poses::CPose3D& b);
    std::tuple<bool, mrpt::poses::CPose3D> check(const mrpt::poses::CPose3D& p) const;

    std::vector<NearbyPose> findNearby(
        const mrpt::poses::CPose3D& p,
        double maxTranslation,
        double maxRotationRad,
        size_t maxCount = 0
        ) const;

    uint32_t countNearby(
        const mrpt::poses::CPose3D& p,
        double maxTranslation,
        double maxRotationRad
        ) const;

    void removeAllFartherThan(
        const mrpt::poses::CPose3D& p,
        double maxTranslation
        );
};

Methods

void insert(const mrpt::poses::CPose3D& p, KFID id)

Same as insert(p), but tags the stored entry with id so that the pose can later be updated in place via setPoseById(). No-op in from_last_only_ mode (the single tracked pose has no id).

void setPoseById(KFID id, const mrpt::poses::CPose3D& new_pose)

Updates the stored pose for an entry previously inserted with an id. No-op if from_last_only_ is set or id is unknown. The internal KD-tree point is updated in place; subsequent NN queries reflect the new pose.

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

Re-expresses every stored pose in a new reference frame, i.e. each stored \(p_i\) becomes \(b \oplus p_i\). Distances between stored poses are preserved, so the “is there a keyframe here already?” decisions are unchanged; only the frame they are expressed in moves.

std::vector<NearbyPose> findNearby(
    const mrpt::poses::CPose3D& p,
    double maxTranslation,
    double maxRotationRad,
    size_t maxCount = 0
    ) const

Returns every stored pose within both the given translation and rotation distance from p, i.e. those satisfying translation(p - candidate).norm() <= maxTranslation && SO3_log(rotation(p - candidate)).norm() <= maxRotationRad.

Gating on rotation as well as translation is what separates a genuine revisit from a pass through the same place on a different heading. The two are not interchangeable for any consumer that then registers the two observations against each other: an error in the body-to-sensor lever arm d enters such a comparison as (R_ij - I) d, which vanishes at equal heading and grows toward a half turn, so a translation-only neighborhood silently mixes a calibration error into the measurement.

Results are ordered by increasing translation distance. Pass a non-zero maxCount to keep only that many of the closest.

In from_last_only_ mode at most one entry is returned, and it carries no id.

uint32_t countNearby(
    const mrpt::poses::CPose3D& p,
    double maxTranslation,
    double maxRotationRad
    ) const

Returns the count of stored poses that are within both the given translation and rotation distance from p. The check is: translation(p - candidate).norm() <= maxTranslation && SO3_log(rotation(p - candidate)).norm() <= maxRotationRad