MAC-VIO

Overview

MAC-VIO (Matching-Aware Covariance Visual-Inertial Odometry) is the state estimation frontend of the FireSense SLAM system. It estimates the 6-DOF pose, velocity, and IMU bias of the robot by tightly coupling deep-learning-based visual odometry with IMU measurements through a GTSAM factor graph.

The system is built on top of MAC-VO, a visual odometry framework that produces per-feature uncertainty estimates from its deep network. MAC-VIO extends this with a full IMU integration pipeline using GTSAM’s incremental Bayes-tree smoother (iSAM2), enabling drift-reduced state estimation suitable for deployment on the NVIDIA Jetson AGX Orin.

Architecture

The pipeline is split into a Python frontend (MAC-VO’s GPU-accelerated deep network) and a C++ backend (GTSAM iSAM2 sliding-window optimizer). They communicate through pybind11 bindings at each keyframe.

Frontend: MAC-VO Feature Tracker

The visual frontend runs on GPU and produces uncertainty-annotated sparse features:

• FlowFormerCov — transformer-based optical flow network that outputs, per tracked feature, the 2D flow vector and a 2×2 covariance matrix representing matching uncertainty.
• StereoNet — estimates per-pixel disparity and depth covariance from the left-right stereo pair.
• Covariance-aware keypoint selector — selects keypoints based on both tracking quality and depth uncertainty bounds (max_match_cov, max_depth_cov). Features with poor depth estimates are kept for tracking continuity but excluded from the optimization.

The frontend runs with a configurable number of keypoints (default: 200) and a maximum tracking age (kp_max_age: 2 frames). Keypoints that age out are re-detected using a covariance-masked uniform grid.

IMU Initialization: Gravity Alignment

Before the GTSAM backend can be started, the world frame must be established. MAC-VIO uses a GravityAlignInitializer that requires no external reference:

1. The system checks whether the platform is approximately stationary using per-frame accelerometer variance (threshold acc_std_thresh) and gyroscope magnitude (threshold gyro_norm_thresh).
2. Accelerometer readings from a rolling window (window_size frames) are averaged to get a stable gravity direction estimate in the body frame.
3. A rotation from the measured gravity vector to the world-frame gravity axis [0, 0, +g] is computed using the cross product and pp.so3 angle-axis representation, giving T_WB0.
4. GTSAM is initialized with this pose, zero initial velocity, and zero bias priors.

If the platform is not stationary during startup, the buffer is reset and initialization retries on subsequent frames, avoiding a corrupted world-frame estimate.

Factor Graph Structure

The factor graph grows incrementally: one new node triplet (X(k), V(k), B(k)) is added per keyframe. The graph is maintained by iSAM2, which relinearizes only affected variables on each update.

 Variables

Symbol	Type	Meaning
X(k)	gtsam::Pose3	Body pose in world frame at keyframe k
V(k)	gtsam::Vector3	World-frame linear velocity at keyframe k
B(k)	imuBias::ConstantBias	Accelerometer + gyroscope bias at keyframe k

GTSAM Implementation

C++ Module: macvo_optim_gtsam

The backend is a pybind11 extension module (Src/CUExt/GTSAM/) that exposes two classes to Python:

SlidingWindowVIO

iSAM2 configuration:

• Relinearize threshold: 0.1 (variables are relinearized when their linearization point changes by more than this)
• Relinearize skip: every 10 updates
• Factorization: Cholesky (faster than QR for dense subgraphs)
• Two update iterations per keyframe for better convergence

Pose covariance extraction:
After each iSAM2 update, the 6×6 marginal covariance of X(k) is extracted via gtsam::Marginals. If the linear system is indeterminate (rank-deficient graph), a fallback diagonal covariance of 0.01 * I is used and covariance_valid = false is reported.

TwoFrameGTSAM

A stateless single-frame pose optimizer used in the non-sliding-window mode. Uses Levenberg-Marquardt with the same ReprojDisparityFactor and Huber robust kernel. Returns the optimized T_CW and its 6×6 marginal covariance.

Key Design Decisions

Why late-fusion instead of tight coupling?
Tight coupling (per-landmark ReprojDisparityFactor in the sliding window) gives theoretically optimal estimates but scales as O(N × W) factors where N is the number of landmarks and W is the window size. For the FireSense deployment with 200 features and a 20-frame window, this would add 4,000 factors per update. Late fusion collapses the visual constraint into a single BetweenFactor<Pose3> — lower graph density at the cost of decoupling the landmark triangulation from IMU constraints. The current default uses late fusion for real-time performance on the Orin.

Why iSAM2 instead of batch optimization?
iSAM2 incrementally updates only the Bayes tree nodes affected by new measurements, giving amortized O(1) complexity for well-conditioned sliding-window problems. Batch re-optimization on every frame would be prohibitive at 10 Hz.

Why a custom ReprojDisparityFactor?
GTSAM’s built-in GenericStereoFactor assumes a standard OpenCV rectified stereo convention. MAC-VO operates in NED camera frame where the depth axis is x (forward), not z. The custom factor encodes the correct projection geometry and handles per-feature heteroscedastic noise directly from the network’s covariance output.