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:
    // typedefs

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

    // structs

    struct FrameState;
    struct GtsamImpl;
    struct State;

    // classes

    class FactorConstVelKinematics;
    class FactorTricycleKinematics;

    // 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;
    StateEstimationSmoother& operator = (const StateEstimationSmoother&);
    StateEstimationSmoother& operator = (StateEstimationSmoother&&);
    void subscribeToLocalizationUpdates(const localization_updates_callback_t& callback);
};

Inherited Members

public:
    // structs

    struct DiagnosticsOutput;
    struct LocalizationUpdate;

    // 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
        );

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
    )

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.