class mola::NavStateFG
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, the first frame of reference will be used as “global” coordinates, despite it may be actually either a
map
orodom
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:
Call initialize() or set the required parameters directly in params_.
Integrate measurements with
fuse_*()
methods. Each CObservation class includes atimestamp
field which is used to estimate the trajectory.
Repeat (2) as needed.
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/
#include <NavStateFG.h> class NavStateFG: public mola::NavStateFilter { public: // structs struct GtsamImpl; struct OdomData; struct PointData; struct PoseData; struct QueryPointData; struct State; struct TwistData; // fields NavStateFGParams params_; // methods virtual void initialize(const mrpt::containers::yaml& cfg); 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_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_frame_ids(); };
Inherited Members
public: // methods virtual void reset() = 0; virtual void initialize(const mrpt::containers::yaml& cfg) = 0; 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_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;
Methods
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 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 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_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_frame_ids()
Returns a list of known frame_ids: