struct mola::state_estimation_smoother::StateEstimationSmoother::State

Overview

struct State
{
    // structs

    struct RawSourcePose;
    struct RelativePoseChain;

    // 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::map<odometry_frameid_t, RelativePoseChain> relative_pose_chains;
    std::map<odometry_frameid_t, mrpt::Clock::time_point> last_kept_pose_stamp;
    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::poses::CPose3DPDFGaussian> wheels_odometry_accumulated;
    std::optional<frame_index_t> last_wheels_odometry_kf;
    std::optional<mrpt::poses::CPose2D> last_wheels_odometry_at_kf;
    std::optional<frame_index_t> wheels_odometry_anchor_kf;
    std::shared_ptr<ImuDecimator> imu_decimator;
    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::map<odometry_frameid_t, mrpt::Clock::time_point> last_kept_pose_stamp

Per fuse_pose() source, the timestamp of the last reading that was allowed through, for Parameters::pose_min_sample_period.

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::poses::CPose3DPDFGaussian> wheels_odometry_accumulated

Absolute dead-reckoned wheel-odometry pose in its own {odom_i} frame, with the uncertainty accumulated over the whole history, for the one factor resolving T_map_to_odom_i and for the source’s own-frame pose estimated_navstate() extrapolates from. The pose is the same one the source reports, and the covariance is the composed motion-model covariance of every increment since the first reading. Reset with the estimator.

std::optional<frame_index_t> last_wheels_odometry_kf

Tail keyframe of the wheel-odometry relative-factor chain. Reset with the estimator.

std::optional<mrpt::poses::CPose2D> last_wheels_odometry_at_kf

Odometry reading representing last_wheels_odometry_kf: the first one that landed on it. The next increment factor starts here, so later readings on the same keyframe lose no motion.

std::optional<frame_index_t> wheels_odometry_anchor_kf

Keyframe carrying the single absolute pose-in-{odom_i} factor that resolves T_map_to_odom_i for wheel odometry. Set once and never renewed: the fixed-lag smoother marginalizes keyframes rather than dropping their factors, so that information survives its own keyframe. See fuse_odometry_relative_locked().

std::shared_ptr<ImuDecimator> imu_decimator

Created on first use. See ImuDecimator.

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.