Class NavStateFG

Nested Relationships

class NavStateFG : public mola::NavStateFilter

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, the first frame of reference will be used as “global” coordinates, despite it may be actually either a map or odom frame, in the ROS REP 105 sense.
IMU readings are, by definition, given in the 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(): TO-DO.
fuse_imu(): TO-DO.

Usage:

(1) Call initialize() or set the required parameters directly in params_.
(2) Integrate measurements with fuse_*() methods. Each CObservation class includes a timestamp field which is used to estimate the trajectory.
(3) Repeat (2) as needed.
(4) 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 the papers cited in https://docs.mola-slam.org/latest/

Main API

void initialize(const mrpt::containers::yaml &cfg) override

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

Parameters:: cfg – a YAML node with a dictionary of parameters to load from.

void fuse_pose(const mrpt::Clock::time_point &timestamp, const mrpt::poses::CPose3DPDFGaussian &pose, const std::string &frame_id) override: Integrates new SE(3) pose estimation of the vehicle wrt frame_id

void fuse_odometry(const mrpt::obs::CObservationOdometry &odom, const std::string &odomName = "odom_wheels") override: 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.

void fuse_imu(const mrpt::obs::CObservationIMU &imu) override: Integrates new IMU observations into the estimator

void fuse_twist(const mrpt::Clock::time_point &timestamp, const mrpt::math::TTwist3D &twist, const mrpt::math::CMatrixDouble66 &twistCov) override: Integrates new twist estimation (in the odom frame)

std::optional<NavState> estimated_navstate(const mrpt::Clock::time_point &timestamp, const std::string &frame_id) override: 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_frame_ids() -> std::set<std::string>: Returns a list of known frame_ids:

Public Functions