{"id":142,"date":"2026-04-03T17:40:19","date_gmt":"2026-04-03T17:40:19","guid":{"rendered":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/?page_id=142"},"modified":"2026-04-04T03:18:28","modified_gmt":"2026-04-04T03:18:28","slug":"mac-vio","status":"publish","type":"page","link":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/system-summary\/mac-vio\/","title":{"rendered":"MAC-VIO"},"content":{"rendered":"\n<iframe loading=\"lazy\" width=\"560\" height=\"315\" src=\"https:\/\/www.youtube.com\/embed\/fGh0QKgVXK8?si=Ok4VSQcb67BLXHuP&amp;start=25\" frameborder=\"0\" allowfullscreen=\"\"><\/iframe>\n\n\n\n<h2 class=\"wp-block-heading\">Overview<\/h2>\n\n\n\n<p>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.<\/p>\n\n\n\n<p>The system is built on top of&nbsp;<a href=\"https:\/\/mac-vo.github.io\/\">MAC-VO<\/a>, 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\u2019s incremental Bayes-tree smoother (iSAM2), enabling drift-reduced state estimation suitable for deployment on the NVIDIA Jetson AGX Orin.<\/p>\n\n\n\n<h2 class=\"wp-block-heading\">Architecture<\/h2>\n\n\n\n<p>The pipeline is split into a&nbsp;<strong>Python frontend<\/strong>&nbsp;(MAC-VO\u2019s GPU-accelerated deep network) and a&nbsp;<strong>C++ backend<\/strong>&nbsp;(GTSAM iSAM2 sliding-window optimizer). They communicate through pybind11 bindings at each keyframe.<\/p>\n\n\n\n<h3 class=\"wp-block-heading\" id=\"frontend%3A-mac-vo-feature-tracker\">Frontend: MAC-VO Feature Tracker<\/h3>\n\n\n\n<p>The visual frontend runs on GPU and produces uncertainty-annotated sparse features:<\/p>\n\n\n\n<ul class=\"wp-block-list\">\n<li><strong>FlowFormerCov<\/strong>\u00a0\u2014 transformer-based optical flow network that outputs, per tracked feature, the 2D flow vector and a 2\u00d72 covariance matrix representing matching uncertainty.<\/li>\n\n\n\n<li><strong>StereoNet<\/strong>\u00a0\u2014 estimates per-pixel disparity and depth covariance from the left-right stereo pair.<\/li>\n\n\n\n<li><strong>Covariance-aware keypoint selector<\/strong>\u00a0\u2014 selects keypoints based on both tracking quality and depth uncertainty bounds (<code>max_match_cov<\/code>,\u00a0<code>max_depth_cov<\/code>). Features with poor depth estimates are kept for tracking continuity but excluded from the optimization.<\/li>\n<\/ul>\n\n\n\n<p>The frontend runs with a configurable number of keypoints (default: 200) and a maximum tracking age (<code>kp_max_age: 2<\/code>&nbsp;frames). Keypoints that age out are re-detected using a covariance-masked uniform grid.<\/p>\n\n\n\n<h3 class=\"wp-block-heading\" id=\"imu-initialization%3A-gravity-alignment\">IMU Initialization: Gravity Alignment<\/h3>\n\n\n\n<p>Before the GTSAM backend can be started, the world frame must be established. MAC-VIO uses a&nbsp;<strong><code>GravityAlignInitializer<\/code><\/strong>&nbsp;that requires no external reference:<\/p>\n\n\n\n<ol class=\"wp-block-list\">\n<li>The system checks whether the platform is approximately stationary using per-frame accelerometer variance (threshold\u00a0<code>acc_std_thresh<\/code>) and gyroscope magnitude (threshold\u00a0<code>gyro_norm_thresh<\/code>).<\/li>\n\n\n\n<li>Accelerometer readings from a rolling window (<code>window_size<\/code>\u00a0frames) are averaged to get a stable gravity direction estimate in the body frame.<\/li>\n\n\n\n<li>A rotation from the measured gravity vector to the world-frame gravity axis\u00a0<code>[0, 0, +g]<\/code>\u00a0is computed using the cross product and\u00a0<code>pp.so3<\/code>\u00a0angle-axis representation, giving\u00a0<code>T_WB0<\/code>.<\/li>\n\n\n\n<li>GTSAM is initialized with this pose, zero initial velocity, and zero bias priors.<\/li>\n<\/ol>\n\n\n\n<p>If the platform is not stationary during startup, the buffer is reset and initialization retries on subsequent frames, avoiding a corrupted world-frame estimate.<\/p>\n\n\n\n<h2 class=\"wp-block-heading\" id=\"factor-graph-structure\">Factor Graph Structure<\/h2>\n\n\n\n<figure class=\"wp-block-image size-full\"><img loading=\"lazy\" decoding=\"async\" width=\"986\" height=\"385\" src=\"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-content\/uploads\/sites\/97\/2026\/04\/MAC-VIO_factor_graph-1.png\" alt=\"\" class=\"wp-image-160\" srcset=\"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-content\/uploads\/sites\/97\/2026\/04\/MAC-VIO_factor_graph-1.png 986w, https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-content\/uploads\/sites\/97\/2026\/04\/MAC-VIO_factor_graph-1-300x117.png 300w, https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-content\/uploads\/sites\/97\/2026\/04\/MAC-VIO_factor_graph-1-768x300.png 768w\" sizes=\"auto, (max-width: 986px) 100vw, 986px\" \/><\/figure>\n\n\n\n<p>The factor graph grows incrementally: one new node triplet&nbsp;<code>(X(k), V(k), B(k))<\/code>&nbsp;is added per keyframe. The graph is maintained by iSAM2, which relinearizes only affected variables on each update.<\/p>\n\n\n\n<h3 class=\"wp-block-heading\" id=\"variables\"><a href=\"http:\/\/localhost:8437\/14#variables\"><\/a>&nbsp;Variables<\/h3>\n\n\n\n<figure class=\"wp-block-table\"><table class=\"has-fixed-layout\"><thead><tr><th>Symbol<\/th><th>Type<\/th><th>Meaning<\/th><\/tr><\/thead><tbody><tr><td><code>X(k)<\/code><\/td><td><code>gtsam::Pose3<\/code><\/td><td>Body pose in world frame at keyframe k<\/td><\/tr><tr><td><code>V(k)<\/code><\/td><td><code>gtsam::Vector3<\/code><\/td><td>World-frame linear velocity at keyframe k<\/td><\/tr><tr><td><code>B(k)<\/code><\/td><td><code>imuBias::ConstantBias<\/code><\/td><td>Accelerometer + gyroscope bias at keyframe k<\/td><\/tr><\/tbody><\/table><\/figure>\n\n\n\n<h2 class=\"wp-block-heading\" id=\"gtsam-implementation\">GTSAM Implementation<\/h2>\n\n\n\n<h3 class=\"wp-block-heading\" id=\"c%2B%2B-module%3A-macvo_optim_gtsam\">C++ Module:\u00a0<code>macvo_optim_gtsam<\/code><\/h3>\n\n\n\n<p>The backend is a pybind11 extension module (<code>Src\/CUExt\/GTSAM\/<\/code>) that exposes two classes to Python:<\/p>\n\n\n\n<h4 class=\"wp-block-heading\" id=\"slidingwindowvio\"><code>SlidingWindowVIO<\/code><\/h4>\n\n\n\n<p><strong>iSAM2 configuration:<\/strong><\/p>\n\n\n\n<ul class=\"wp-block-list\">\n<li>Relinearize threshold: 0.1 (variables are relinearized when their linearization point changes by more than this)<\/li>\n\n\n\n<li>Relinearize skip: every 10 updates<\/li>\n\n\n\n<li>Factorization: Cholesky (faster than QR for dense subgraphs)<\/li>\n\n\n\n<li>Two update iterations per keyframe for better convergence<\/li>\n<\/ul>\n\n\n\n<p><strong>Pose covariance extraction:<\/strong><br>After each iSAM2 update, the 6\u00d76 marginal covariance of&nbsp;<code>X(k)<\/code>&nbsp;is extracted via&nbsp;<code>gtsam::Marginals<\/code>. If the linear system is indeterminate (rank-deficient graph), a fallback diagonal covariance of&nbsp;<code>0.01 * I<\/code>&nbsp;is used and&nbsp;<code>covariance_valid = false<\/code>&nbsp;is reported.<\/p>\n\n\n\n<h4 class=\"wp-block-heading\" id=\"twoframegtsam\"><code>TwoFrameGTSAM<\/code><\/h4>\n\n\n\n<p>A stateless single-frame pose optimizer used in the non-sliding-window mode. Uses Levenberg-Marquardt with the same&nbsp;<code>ReprojDisparityFactor<\/code>&nbsp;and Huber robust kernel. Returns the optimized&nbsp;<code>T_CW<\/code>&nbsp;and its 6\u00d76 marginal covariance.<\/p>\n\n\n\n<h2 class=\"wp-block-heading\" id=\"key-design-decisions\">Key Design Decisions<\/h2>\n\n\n\n<p><strong>Why late-fusion instead of tight coupling?<\/strong><br>Tight coupling (per-landmark&nbsp;<code>ReprojDisparityFactor<\/code>&nbsp;in the sliding window) gives theoretically optimal estimates but scales as O(N \u00d7 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&nbsp;<code>BetweenFactor&lt;Pose3&gt;<\/code>&nbsp;\u2014 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.<\/p>\n\n\n\n<p><strong>Why iSAM2 instead of batch optimization?<\/strong><br>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.<\/p>\n\n\n\n<p><strong>Why a custom&nbsp;<code>ReprojDisparityFactor<\/code>?<\/strong><br>GTSAM\u2019s built-in&nbsp;<code>GenericStereoFactor<\/code>&nbsp;assumes a standard OpenCV rectified stereo convention. MAC-VO operates in NED camera frame where the depth axis is&nbsp;<code>x<\/code>&nbsp;(forward), not&nbsp;<code>z<\/code>. The custom factor encodes the correct projection geometry and handles per-feature heteroscedastic noise directly from the network\u2019s covariance output.<\/p>\n","protected":false},"excerpt":{"rendered":"<p>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&nbsp;MAC-VO, a visual odometry framework that produces [&hellip;]<\/p>\n","protected":false},"author":458,"featured_media":0,"parent":2,"menu_order":0,"comment_status":"closed","ping_status":"closed","template":"page-with-sidebar","meta":{"footnotes":""},"class_list":["post-142","page","type-page","status-publish","hentry"],"_links":{"self":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-json\/wp\/v2\/pages\/142","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-json\/wp\/v2\/pages"}],"about":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-json\/wp\/v2\/types\/page"}],"author":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-json\/wp\/v2\/users\/458"}],"replies":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-json\/wp\/v2\/comments?post=142"}],"version-history":[{"count":2,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-json\/wp\/v2\/pages\/142\/revisions"}],"predecessor-version":[{"id":161,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-json\/wp\/v2\/pages\/142\/revisions\/161"}],"up":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-json\/wp\/v2\/pages\/2"}],"wp:attachment":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teamf\/wp-json\/wp\/v2\/media?parent=142"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}