class mola::state_estimation_smoother::StateEstimationSmoother

Overview

Sliding window Factor-graph data fusion for odometry, IMU, GNSS, and SE(3) pose/twist estimations.

Frame conventions:

  • There is a frame of reference for each source of odometry, e.g. there may be one for LiDAR-odometry, another for visual-odometry, or wheels-based odometry, etc. Each such frame is referenced with a “frame

    name” (an arbitrary string).

  • Internally, this class uses the {utm}, {enu}, and {map} frames. Refer to the frame diagrams here.

  • The name for the reference frame (Default: "map") and the robot/vehicle ("base_link") can be changed from the parameters (e.g. the config yaml file).

  • This package DOES NOT follow the ROS REP 105 specifications in the sense that /tf from {map} → {odom} are not published. Instead, it directly emits {map} → {base_link} from the fusion of all available data.

  • Publishing the vehicle pose in a timely manner uses “params.reference_frame_name” as reference frame.

  • IMU readings are, by definition, given in the local robot body frame, although they can have a relative transformation between the vehicle and sensor.

Main API methods and frame conventions:

  • estimated_navstate() : Output estimations can be requested in any of the existing frames of reference.

  • fuse_pose() : Can be used to integrate information from any “odometry” or “localization” input, as mentioned above.

  • fuse_gnss() : Integrate GNSS observations, to help with localization in geo-referenced maps, or to automatically find-out the geo-referencing of a map.

  • fuse_imu() : Used to help with (1) global azimuth in geo-referenced maps, (2) vertical direction from accelerometer, (3) angular velocity from gyroscope.

Usage:

    1. Call initialize() to set the required parameters.

    1. Integrate measurements with fuse_*() methods. Each CObservation class includes a timestamp field which is used to estimate the trajectory.

    1. Repeat (2) as needed.

    1. Read the estimation up to any nearby moment in time with estimated_navstate()

Old observations are automatically removed.

A constant SE(3) velocity model is internally used, without any particular assumptions on the vehicle kinematics.

For more theoretical descriptions, see: https://docs.mola-slam.org/latest/mola_state_estimators.html

#include <StateEstimationSmoother.h>

class StateEstimationSmoother:
    public mola::NavStateFilter,
    public mola::LocalizationSourceBase,
    public mola::MapSourceBase
{
public:
    // typedefs

    typedef std::function<void(const LocalizationUpdate&)> localization_updates_callback_t;
    typedef std::function<void(const MapUpdate&)> map_updates_callback_t;

    // structs

    struct FrameState;
    struct GtsamImpl;
    struct State;

    // classes

    class AbsFactorConstVelKinematics;
    class AbsFactorTricycleKinematics;

    // construction

    StateEstimationSmoother();
    StateEstimationSmoother(const StateEstimationSmoother&);
    StateEstimationSmoother(StateEstimationSmoother&&);

    // methods

    const Parameters& parameters();
    virtual void initialize(const mrpt::containers::yaml& cfg);
    virtual void spinOnce();
    virtual void reset();

    virtual void fuse_pose(
        const mrpt::Clock::time_point& timestamp,
        const mrpt::poses::CPose3DPDFGaussian& pose,
        const std::string& frame_id
        );

    virtual void fuse_odometry(
        const mrpt::obs::CObservationOdometry& odom,
        const std::string& odomName = "odom_wheels"
        );

    virtual void fuse_imu(const mrpt::obs::CObservationIMU& imu);
    virtual void fuse_gnss(const mrpt::obs::CObservationGPS& gps);

    virtual void fuse_twist(
        const mrpt::Clock::time_point& timestamp,
        const mrpt::math::TTwist3D& twist,
        const mrpt::math::CMatrixDouble66& twistCov
        );

    virtual std::optional<NavState> estimated_navstate(
        const mrpt::Clock::time_point& timestamp,
        const std::string& frame_id
        );

    auto known_odometry_frame_ids();
    std::optional<mrpt::poses::CPose3DPDFGaussian> estimated_T_enu_to_map() const;
    std::optional<mrpt::poses::CPose3DPDFGaussian> estimated_T_map_to_odometry_frame(const std::string& frame_id) const;
    bool has_converged_localization(mrpt::poses::CPose3DPDFGaussian& pose) const;
    std::optional<mola::Georeferencing> current_georeferencing() const;
    size_t count_const_vel_factors_for_testing() const;
    std::set<std::pair<mrpt::Clock::time_point, mrpt::Clock::time_point>> const_vel_factor_links_for_testing() const;
    StateEstimationSmoother& operator = (const StateEstimationSmoother&);
    StateEstimationSmoother& operator = (StateEstimationSmoother&&);
    virtual void onNewObservation(const CObservation::ConstPtr& o);
    void subscribeToLocalizationUpdates(const localization_updates_callback_t& callback);
};

Inherited Members

public:
    // structs

    struct DiagnosticsOutput;
    struct LocalizationUpdate;
    struct MapUpdate;

    // methods

    virtual void initialize(const Yaml& cfg) = 0;
    virtual void spinOnce() = 0;
    ExecutableBase& operator = (const ExecutableBase&);
    ExecutableBase& operator = (ExecutableBase&&);
    virtual void reset() = 0;
    virtual void initialize(const Yaml& cfg);

    virtual void fuse_pose(
        const mrpt::Clock::time_point& timestamp,
        const mrpt::poses::CPose3DPDFGaussian& pose,
        const std::string& frame_id
        ) = 0;

    virtual void fuse_odometry(
        const mrpt::obs::CObservationOdometry& odom,
        const std::string& odomName = "odom_wheels"
        ) = 0;

    virtual void fuse_imu(const mrpt::obs::CObservationIMU& imu) = 0;
    virtual void fuse_gnss(const mrpt::obs::CObservationGPS& gps) = 0;

    virtual void fuse_twist(
        const mrpt::Clock::time_point& timestamp,
        const mrpt::math::TTwist3D& twist,
        const mrpt::math::CMatrixDouble66& twistCov
        ) = 0;

    virtual std::optional<NavState> estimated_navstate(
        const mrpt::Clock::time_point& timestamp,
        const std::string& frame_id
        ) = 0;

    virtual std::optional<mrpt::poses::CPose3DInterpolator> estimated_trajectory(
        ] const mrpt::Clock::time_point& start_time,
        ] const mrpt::Clock::time_point& end_time,
        ] const std::string& frame_id
        );

    virtual bool has_converged_localization(] mrpt::poses::CPose3DPDFGaussian& pose_in_map) const;
    virtual void set_geo_reference(] const mola::Georeferencing& georef);
    virtual std::optional<mola::Georeferencing> get_geo_reference() const;

Methods

const Parameters& parameters()

Parameters can only be set via initialize(), then read-only accesses through this method.

virtual void initialize(const mrpt::containers::yaml& cfg)

Initializes the object and reads all parameters from a YAML node.

Parameters:

cfg

a YAML node with a dictionary of parameters to load from.

 
virtual void spinOnce()

Runs any required action on a timely manner

virtual void reset()

Resets the estimator state to an initial state

virtual void fuse_pose(
    const mrpt::Clock::time_point& timestamp,
    const mrpt::poses::CPose3DPDFGaussian& pose,
    const std::string& frame_id
    )

Integrates new SE(3) pose odometry estimation of the vehicle wrt frame_id

virtual void fuse_odometry(
    const mrpt::obs::CObservationOdometry& odom,
    const std::string& odomName = "odom_wheels"
    )

Integrates new wheels-based odometry observations into the estimator. This is a convenience method that internally ends up calling fuse_pose(), but computing the uncertainty of odometry increments according to a given motion model.

virtual void fuse_imu(const mrpt::obs::CObservationIMU& imu)

Integrates new IMU observations into the estimator

virtual void fuse_gnss(const mrpt::obs::CObservationGPS& gps)

Integrates new GNSS observations into the estimator

virtual void fuse_twist(
    const mrpt::Clock::time_point& timestamp,
    const mrpt::math::TTwist3D& twist,
    const mrpt::math::CMatrixDouble66& twistCov
    )

Anchors T_enu_to_map to a known, fixed value (equivalent to setting the fixed_geo_reference YAML parameter, but at runtime): used by localization front-ends (e.g. mola_lidar_odometry) once they load a geo-referenced map, so GNSS fixes constrain the vehicle’s position in that map’s frame without the estimator also trying to solve for T_enu_to_map itself (that combination leaves T_enu_to_map and every fused Pose3RotationFactor/MeasuredGravityFactor-derived orientation jointly underconstrained, since only their relative rotation is ever measured). Re-initializes the smoother’s factor graph, so it should be called before any run-relevant sensor fusion, not mid-run. Integrates new twist estimation (in the odom frame)

virtual std::optional<NavState> estimated_navstate(
    const mrpt::Clock::time_point& timestamp,
    const std::string& frame_id
    )

Computes the estimated vehicle state at a given timestep using the observations in the time window. A std::nullopt is returned if there is no valid observations yet, or if requested a timestamp out of the model validity time window (e.g. too far in the future to be trustful).

auto known_odometry_frame_ids()

Returns a list of known odometry frame_ids:

std::optional<mrpt::poses::CPose3DPDFGaussian> estimated_T_enu_to_map() const

Gets the latest estimated transform of T_enu_to_map.

std::optional<mrpt::poses::CPose3DPDFGaussian> estimated_T_map_to_odometry_frame(const std::string& frame_id) const

Gets the latest estimated transform of “T_map_to_odometry_frame_i”, by frame ID name.

bool has_converged_localization(mrpt::poses::CPose3DPDFGaussian& pose) const

Implements NavStateFilter::has_converged_localization.

std::optional<mola::Georeferencing> current_georeferencing() const

Returns the current georeferencing, if available.

size_t count_const_vel_factors_for_testing() const

Number of constant-velocity kinematic factors currently live in the underlying factor graph (2 per kinematic link).

For tests only. It counts the real GTSAM graph rather than this class’s own link bookkeeping, so that a test can tell whether a factor removal actually reached the solver. Returns a plain count so GTSAM stays out of this header.

std::set<std::pair<mrpt::Clock::time_point, mrpt::Clock::time_point>> const_vel_factor_links_for_testing() const

Identifies each constant-velocity kinematic link currently live in the underlying factor graph by its endpoint timestamps, one entry per link (not per factor: each link emits two factors, linear and angular).

For tests only, to check the graph topology survives a splice as expected (e.g. that the direct a->b link was replaced by a->n and n->b, not left alongside them).

virtual void onNewObservation(const CObservation::ConstPtr& o)

To be called whenever a new observation arrives. It should return as fast as possible, enqueuing the data for processing in another thread.