struct mola::LidarOdometry::MethodState::GravityEstimator

Overview

Accumulates recent accelerometer readings and provides a smoothed pitch/roll estimate from the gravity direction.

#include <LidarOdometry.h>

struct GravityEstimator
{
    // structs

    struct TimestampedAcc;

    // fields

    mrpt::containers::circular_buffer<TimestampedAcc> acc_buffer {IMU_BUFFER_SIZE};
    mrpt::poses::CPose3D imu_sensor_pose;
    double imu_sensor_pose_timestamp = 0;

    // methods

    void add(
        const mrpt::obs::CObservationIMU& imu,
        uint32_t max_samples
        );

    std::optional<std::pair<double, double>> estimatedPitchRoll(
        uint32_t required_samples,
        double max_age_seconds
        ) const;

    std::optional<double> directionDispersionSigma(double max_age_seconds) const;
};

Fields

mrpt::poses::CPose3D imu_sensor_pose

last known IMU extrinsics

double imu_sensor_pose_timestamp = 0

timestamp of last sensor pose update

Methods

std::optional<std::pair<double, double>> estimatedPitchRoll(
    uint32_t required_samples,
    double max_age_seconds
    ) const

Returns estimated (pitch, roll) in radians from averaged accelerometer data in the vehicle frame, or nullopt if not enough data. max_age_seconds <= 0 means no age filtering.

std::optional<double> directionDispersionSigma(double max_age_seconds) const

Empirical 1-sigma [rad] of the gravity DIRECTION over the samples currently in the buffer: the RMS angle between each buffered sample’s direction and their mean direction.

This is the honest uncertainty of the reading, measured rather than assumed. While quasi-static it is small; under real vehicle dynamics (braking, cornering, vibration) the accepted samples disagree and it grows, so a caller that adds it in quadrature to its configured sigma gets a verticality constraint that self-silences exactly when the quasi-static assumption behind it stops holding.

nullopt if fewer than 2 usable samples.