Class SearchablePoseList

Class Documentation

class SearchablePoseList

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.

Public Types

using KFID = uint64_t

Public Functions

SearchablePoseList() = default
inline SearchablePoseList(bool measure_from_last_kf_only)
inline bool empty() const
inline size_t size() const
inline void insert(const mrpt::poses::CPose3D &p)
inline 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.

std::tuple<bool, mrpt::poses::CPose3D> check(const mrpt::poses::CPose3D &p) const
void removeAllFartherThan(const mrpt::poses::CPose3D &p, double maxTranslation)