How paulkemppi/gtsam_fusion works
gtsam_fusion is a ROS/GTSAM state-estimation package: IMU data drives pose, velocity, and bias prediction, while GPS and/or 6DOF pose measurements constrain drift through incremental ISAM2 optimization. The ROS node supports live topic subscriptions and rosbag playback, converts frames and GPS coordinates, then publishes the fused base pose; optional plotting compares inputs and estimates. The graph follows documented behavior and sampled source, but ROS bag dispatch and some conversion details are only partially excerpted.
Main entry points: gtsam_fusion_ros.py ROS fusion node · gtsam_fusion_core.py Fusion core · plots.py Result plots
Components 19
ROS Interface
- ROS fusion node
scripts/gtsam_fusion_ros.py - Rosbag reader
scripts/gtsam_fusion_ros.py - GPS to ENU
scripts/gtsam_fusion_ros.py - Frame transforms
scripts/gtsam_fusion_ros.py - Pose publisher
scripts/gtsam_fusion_ros.py
Fusion Engine
- Fusion core
scripts/gtsam_fusion_core.py - IMU preintegration
scripts/gtsam_fusion_core.py - Measurement factors
scripts/gtsam_fusion_core.py - ISAM2 optimizer
scripts/gtsam_fusion_core.py - Pose estimate
scripts/gtsam_fusion_core.py
Output & Analysis
- Result plots
scripts/plots.py
Other components
- Operator
- IMU stream
- GPS stream
- 6DOF pose stream
- Rosbag input
- GTSAM
- ROS pose topic
- Plot viewer
Connections 18
- Operator → ROS fusion node: starts
- Rosbag input → Rosbag reader: provides data
- Rosbag reader → ROS fusion node: dispatches messages
- IMU stream → ROS fusion node: publishes IMU
- GPS stream → ROS fusion node: publishes GPS
- 6DOF pose stream → ROS fusion node: publishes pose
- ROS fusion node → GPS to ENU: converts GPS
- ROS fusion node → Frame transforms: transforms frames
- ROS fusion node → Fusion core: submits measurements
- Fusion core → IMU preintegration: preintegrates
- Fusion core → Measurement factors: adds constraints
- Fusion core → ISAM2 optimizer: updates graph
- ISAM2 optimizer → GTSAM: optimizes with
- Fusion core → Pose estimate: returns state
- ROS fusion node → Pose publisher: publishes estimate
- Pose publisher → ROS pose topic: publishes pose
- ROS fusion node → Result plots: sends recorded data
- Result plots → Plot viewer: displays charts