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;

    // 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);
    std::tuple<bool, mrpt::poses::CPose3D> check(const mrpt::poses::CPose3D& p) 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.

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