class mola::state_estimation_smoother::FastPredictor

Overview

Serves estimated_navstate() queries in sub-millisecond time, without touching the smoother’s stateMutex_ or running any factor-graph solve.

It holds the latest immutable Snapshot published by the backend and, on each query, extrapolates that anchor to the requested time with the configured kinematic model (identical maths to the synchronous StateEstimationSmoother::estimated_navstate(), so the two agree up to the backend’s solve lag). The frame-local {odom_i} hardening is preserved: an odometry-frame query anchors on that source’s own last raw pose, never on a {map}-frame reconstruction.

Thread-safety: its own mutex, deliberately separate from the smoother’s stateMutex_, so a query never blocks on a backend batch solve.

#include <FastPredictor.h>

class FastPredictor
{
public:
    // methods

    void set_snapshot(std::shared_ptr<const Snapshot> snap);
    std::shared_ptr<const Snapshot> snapshot() const;
    void note_observation_stamp(const mrpt::Clock::time_point& obsStamp);
    std::optional<mrpt::Clock::time_point> get_current_extrapolated_stamp() const;

    std::optional<NavState> predict(
        const mrpt::Clock::time_point& t_query,
        const std::string& frame_id,
        const Parameters& params
        ) const;

    void clear();
};

Methods

void set_snapshot(std::shared_ptr<const Snapshot> snap)

Publishes a newly solved snapshot (called by the backend after a solve).

std::shared_ptr<const Snapshot> snapshot() const

Returns the current snapshot (may be null / invalid before the first solve).

void note_observation_stamp(const mrpt::Clock::time_point& obsStamp)

Records the timestamp of the latest incoming observation together with the current wallclock, so a real-time “now” stamp can be computed without touching the smoother’s stateMutex_.

std::optional<mrpt::Clock::time_point> get_current_extrapolated_stamp() const

Real-time “now” stamp: the latest observation stamp advanced by the wallclock elapsed since it was received. nullopt if none seen yet.

std::optional<NavState> predict(
    const mrpt::Clock::time_point& t_query,
    const std::string& frame_id,
    const Parameters& params
    ) const

Extrapolates the snapshot anchor to t_query in frame_id. nullopt if there is no valid snapshot, the frame is unknown, or t_query is beyond max_time_to_use_velocity_model.

void clear()

Drops the snapshot and the observation-stamp tracking (used on reset()).