struct mola::state_estimation_smoother::StateEstimationSmoother::State

Overview

struct State
{
    // structs

    struct RawSourcePose;

    // fields

    mrpt::pimpl<GtsamImpl> gtsam;
    frame_index_t next_frame_index = 0;
    mrpt::containers::bimap<mrpt::Clock::time_point, frame_index_t> stamp2frame_index;
    mrpt::containers::bimap<std::string, odometry_frameid_t> known_odom_frames;
    odometry_frameid_t next_odom_frame_id = 1;
    std::map<frame_index_t, FrameState> last_estimated_states;
    std::optional<mrpt::math::TTwist3D> filtered_predict_twist;
    std::optional<mrpt::Clock::time_point> filtered_predict_twist_stamp;
    std::map<odometry_frameid_t, mrpt::poses::CPose3DPDFGaussian> last_estimated_frames;
    std::map<odometry_frameid_t, RawSourcePose> last_raw_pose_by_source;
    std::optional<mrpt::Clock::time_point> last_observation_stamp;
    mrpt::Clock::time_point last_observation_wallclock_stamp;
    std::optional<mrpt::poses::CPose2D> last_wheels_odometry;
    std::optional<std::string> last_wheels_odometry_name;
    std::optional<mrpt::Clock::time_point> last_wheels_odometry_stamp;
    std::optional<mrpt::Clock::time_point> last_processed_imu_stamp;
    std::optional<mola::Georeferencing> geo_reference;
    std::optional<mrpt::topography::TGeodeticCoords> tentative_geo_coord_reference;
    bool estimated_georef_published = false;
    RegexCache do_process_imu_labels_re;
    RegexCache do_process_odometry_labels_re;
    RegexCache do_process_gnss_labels_re;

    // methods

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

Fields

frame_index_t next_frame_index = 0

The next numeric ID to assign to a new frame, for usage in GTSAM symbols P(i), v(i)…

mrpt::containers::bimap<mrpt::Clock::time_point, frame_index_t> stamp2frame_index

A bimap of timestamps <=> frame indices. Updated by.

mrpt::containers::bimap<std::string, odometry_frameid_t> known_odom_frames

A bimap of known odometry “frame_id” <=> “numeric IDs”:

odometry_frameid_t next_odom_frame_id = 1

Monotonic counter for odometry frame IDs; avoids collisions if entries are ever removed.

std::map<frame_index_t, FrameState> last_estimated_states

The latest values from the estimator; updated in process_pending_gtsam_updates()

std::optional<mrpt::math::TTwist3D> filtered_predict_twist

Low-pass-filtered twist of the newest keyframe, used as the extrapolation velocity for short-term prediction when predict_twist_filter_enabled. Damps the boundary keyframe’s raw velocity (the least-constrained node), which otherwise injects jitter into a front end’s motion prior. Reset with the rest of State.

std::map<odometry_frameid_t, mrpt::poses::CPose3DPDFGaussian> last_estimated_frames

The latest values from the estimator; updated in process_pending_gtsam_updates()

std::optional<mrpt::Clock::time_point> last_wheels_odometry_stamp

Stamp of the last wheel-odometry reading that was actually KEPT (turned into a keyframe/factor). Used by odometry_min_sample_period to merge higher-rate readings: while a reading arrives within that period of this stamp it is dropped without advancing the pose anchor (last_wheels_odometry), so the next kept reading fuses the whole accumulated increment.

std::optional<mrpt::Clock::time_point> last_processed_imu_stamp

Stamp of the last IMU reading that was actually processed. Used by imu_min_sample_period to skip higher-rate readings.

std::optional<mola::Georeferencing> geo_reference

Refer to Parameters for possible sources of this. Anyways: this will always hold either the estimated or the fixed (externally set) georeferencing parameters. When this is still empty, it means we are still waiting for someone external to send us the georeferencing data, or our internal estimator didn’t obtained a quality estimation yet.

std::optional<mrpt::topography::TGeodeticCoords> tentative_geo_coord_reference

Will be populated with the first GNSS coords when in active estimation mode.

bool estimated_georef_published = false

Flag to track if we’ve already published the estimated geo-ref.

Methods

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

For real-time mode operation (not offline): returns the current extrapolated stamp, by adding the difference between the last observation wallclock time and now to the last observation timestamp.