class mola::GravityMapAligner

Overview

Per-keyframe accelerometer pool + robust gravity-direction estimator.

Samples are keyed by an external KeyFrameID (opaque uint64_t): the KeyframePointCloudMap id when used from LIO, or any integer in unit tests.

Thread-safety: none. Callers serialize access externally.

#include <GravityMapAligner.h>

class GravityMapAligner
{
public:
    // typedefs

    typedef uint64_t KeyFrameID;

    // structs

    struct KFAccSample;
    struct Params;

    // fields

    static constexpr double kDefaultAxisEps = 1e-9;

    // construction

    GravityMapAligner();
    GravityMapAligner(Params p);

    // methods

    const Params& params() const;
    void setParams(Params p);
    void onNewKeyframe(KeyFrameID id, const mola::imu::LocalVelocityBuffer::SamplesByTime& window);
    void onNewKeyframeRaw(KeyFrameID id, const KFAccSample& s);
    void onKeyframeDropped(KeyFrameID id);
    void clear();
    std::size_t size() const;
    bool has(KeyFrameID id) const;
    const std::map<KeyFrameID, KFAccSample>& samples() const;
    std::optional<mrpt::math::TVector3D> estimateGravityVector(const std::map<KeyFrameID, mrpt::poses::CPose3D>& kf_poses) const;

    std::optional<mrpt::math::TVector3D> estimateGravityVectorOverWindow(
        const std::map<KeyFrameID, mrpt::poses::CPose3D>& kf_poses,
        const std::vector<KeyFrameID>& id_window,
        std::size_t min_pool
        ) const;

    std::optional<mrpt::poses::CPose3D> estimateGlobalCorrection(const std::map<KeyFrameID, mrpt::poses::CPose3D>& kf_poses) const;

    std::map<KeyFrameID, mrpt::poses::CPose3D> estimatePerKFCorrection(
        const std::map<KeyFrameID, mrpt::poses::CPose3D>& kf_poses,
        std::size_t window_half_width,
        std::size_t min_pool_per_kf
        ) const;

    static mrpt::poses::CPose3D rotationFromGravity(
        const mrpt::math::TVector3D& g_hat,
        double axis_eps = kDefaultAxisEps
        );
};

Fields

static constexpr double kDefaultAxisEps = 1e-9

Default numerical guard for axis-angle degeneracies in rotationFromGravity / Params::axis_eps.

Methods

void onNewKeyframe(
    KeyFrameID id,
    const mola::imu::LocalVelocityBuffer::SamplesByTime& window
    )

Register a KF and the accelerometer samples taken during its interval. Computes and stores, from the samples in window, the time-averaged body-frame acceleration a_body and the intra-KF std of |a|. If window contains no accelerometer samples (e.g., empty), the KF is NOT added to the pool.

void onNewKeyframeRaw(KeyFrameID id, const KFAccSample& s)

Insert a pre-computed sample directly (useful for tests / rehydration).

void onKeyframeDropped(KeyFrameID id)

Drop per-KF data when a KF is evicted from the local map.

void clear()

Reset all state.

std::optional<mrpt::math::TVector3D> estimateGravityVector(const std::map<KeyFrameID, mrpt::poses::CPose3D>& kf_poses) const

Robust weighted mean of the gravity direction in the “map” frame, pooled over all currently-stored KFs whose ids are also present in kf_poses.

Returns:

The estimated gravity vector (not normalized) in the kf_poses frame, or nullopt if pool size is below params_.min_keyframes.

std::optional<mrpt::math::TVector3D> estimateGravityVectorOverWindow(
    const std::map<KeyFrameID, mrpt::poses::CPose3D>& kf_poses,
    const std::vector<KeyFrameID>& id_window,
    std::size_t min_pool
    ) const

Robust weighted mean of gravity, pooled over the subset of ids in id_window that are both present in this aligner and in kf_poses. Returns nullopt if fewer than min_pool KFs in the intersection (or if <2 samples, which would make the IRLS ill-conditioned).

std::optional<mrpt::poses::CPose3D> estimateGlobalCorrection(const std::map<KeyFrameID, mrpt::poses::CPose3D>& kf_poses) const

One-shot pool-wide correction: pitch+roll rotation sending the robust pool mean of R_i · a_i (in the kf_poses frame) to +Z. nullopt if pool is below trigger size.

std::map<KeyFrameID, mrpt::poses::CPose3D> estimatePerKFCorrection(
    const std::map<KeyFrameID, mrpt::poses::CPose3D>& kf_poses,
    std::size_t window_half_width,
    std::size_t min_pool_per_kf
    ) const

For each KF id in kf_poses that also has pool samples, compute a per-KF pitch+roll rotation R_grav_i (sending the windowed robust mean of R_j · a_j for j in Window(i) to +Z). window_half_width is P in Window(i) = [i-P, i+P] (truncated at ends).

Parameters:

min_pool_per_kf

Minimum number of pool members in Window(i) required to emit R_grav_i (else KF is skipped).

Returns:

map from KF id to R_grav_i.

static mrpt::poses::CPose3D rotationFromGravity(
    const mrpt::math::TVector3D& g_hat,
    double axis_eps = kDefaultAxisEps
    )

Builds the pitch+roll-only rotation that sends the unit vector g_hat to +Z. Yaw is left at zero (unobservable with gravity only). Returns identity if g_hat is already aligned with +Z within axis_eps.