Source-linked AI summary
Super Odometry: IMU-centric LiDAR-Visual-Inertial Estimator for Challenging Environments
Shibo Zhao, Hengrui Zhang, Peng Wang, Lucas Nogueira, Sebastian Scherer
TL;DR
Perceptually degraded environments challenge reliable state estimation because reliable measurements may be unavailable. Super Odometry addresses this with an IMU-centric fusion pipeline combining IMU, visual-inertial, and LiDAR-inertial odometry, and reports robust, high-precision motion estimation in challenging real-world scenarios.
Problem
Perceptually degraded environments lack reliable measurements, making robust state estimation challenging for autonomous robots.
Method
Super Odometry uses an IMU-centric pipeline in which visual-inertial and LiDAR-inertial odometry constrain IMU bias, while IMU odometry predicts motion and provides final state estimation.
Results
The framework was tested in visually and geometrically degraded environments and achieved robust, high-precision, resilient motion estimation in challenging real-world scenarios.
Takeaways & Limitations
The system supports robust state estimation despite individual sensor failures and was deployed on drones and ground robots.
Abstract
from arXiv · showhide
We propose Super Odometry, a high-precision multi-modal sensor fusion framework, providing a simple but effective way to fuse multiple sensors such as LiDAR, camera, and IMU sensors and achieve robust state estimation in perceptually-degraded environments. Different from traditional sensor-fusion methods, Super Odometry employs an IMU-centric data processing pipeline, which combines the advantages of loosely coupled methods with tightly coupled methods and recovers motion in a coarse-to-fine manner. The proposed framework is composed of three parts: IMU odometry, visual-inertial odometry, and laser-inertial odometry. The visual-inertial odometry and laser-inertial odometry provide the pose prior to constrain the IMU bias and receive the motion prediction from IMU odometry. To ensure high performance in real-time, we apply a dynamic octree that only consumes 10 % of the running time compared with a static KD-tree. The proposed system was deployed on drones and ground robots, as part of Team Explorer's effort to the DARPA Subterranean Challenge where the team won $1^{st}$ and $2^{nd}$ place in the Tunnel and Urban Circuits, respectively.
I. INTRODUCTION
Super Odometry addresses unreliable localization in GPS-denied, perceptually degraded environments with an IMU-centric multi-modal fusion design. It combines complementary coupling strategies and an efficient dynamic octree for real-time operation.
- Darkness, fog, smoke, dust, and structure-less scenes make reliable autonomous localization difficult because available measurements become unreliable.
- The pipeline combines tightly coupled accuracy and robustness with loosely coupled simplicity, extendibility, and lower computational expense.
- Super Odometry uses the IMU as the primary sensor because smooth, low-outlier measurements can remain accurate when other sensors constrain bias drift.
- A dynamic octree organizes 3D points to improve scan-matching real-time performance.
- The system was deployed on drones and ground robots and evaluated under aggressive motion, low light, long corridors, and heavy dust.
A. Loosely Coupled LiDAR-Visual-Inertial Odometry
The proposed architecture uses an IMU-centric estimator to combine multiple odometry modules while retaining extensibility and fail-safe behavior. It recovers motion from coarse to fine using IMU prediction and efficient point organization.
- Super Odometry uses multiple state-estimation engines, allowing additional sensors to be fused through pose constraints and helping overcome potential sensor failures.
- The architecture divides a large factor graph into sub-factor-graphs that recover motion from coarse to fine in parallel.
- A dynamic octree makes scan matching efficient by organizing the 3D points used for correspondence search.
- The system comprises IMU odometry, visual-inertial odometry, and LiDAR-inertial odometry, with the IMU serving as the primary sensor.
- Visual-inertial and LiDAR-inertial odometry constrain accelerometer and gyroscope biases, while constrained IMU odometry supplies motion prediction to the other modules.
IV. METHODOLOGY
Super Odometry models IMU, LiDAR-inertial, and visual-inertial odometry factors in a factor graph. IMU preintegration connects consecutive states, while other sensors provide constraints and jointly optimized bias terms.
- The factor graph contains IMU, LiDAR-inertial, and visual-inertial odometry factors.
- IMU odometry states include position, accelerometer bias, and gyroscope bias, with edges representing relative body motion from IMU preintegration.Node density is determined by the lowest-frequency odometry, and additional edges may provide local or global constraints.
- IMU preintegration obtains relative motion between consecutive body frames and defines the corresponding factor.
- Rotation matrices and Hamilton quaternions represent rotation, while translation, velocity, gravity, and sensor biases define the IMU residuals.Accelerometer and gyroscope bias errors are jointly optimized in the graph.
2) Relative Pose Factor:
The relative pose factor uses camera and LiDAR relative poses as local constraints because their odometry is referenced to the robot’s first pose. These constraints act on IMU preintegration position.
- Camera and LiDAR odometry provide relative poses because neither sensor is globally referenced.
- The relative pose measurements are used as local constraints on the IMU preintegration position.
- The LiDAR relative pose factor is derived alongside the visual-inertial relative pose factor.
3) IMU Odometry Optimization:
The optimization processes relative constraints and marginalization priors, weighting visual-inertial and LiDAR-inertial factors according to observation reliability. IMU preintegration also predicts motion for LiDAR scan-map matching.
- Each new frame is optimized using relative constraints and a marginalization prior.
- Visual-inertial factors receive lower weight in visually degraded environments, while LiDAR-inertial factors receive lower weight in geometrically degraded environments.
- IMU odometry can incorporate additional sensors such as GPS and wheel odometry.
- IMU preintegration provides motion prediction for current scan-map matching and connects consecutive LiDAR frames in the factor graph.
- The scan-map matching pipeline comprises PCA-based feature extraction, multi-metric linear-square ICP, a LiDAR-inertial factor, and a dynamic octree.
- PCA analyzes local linearity, planarity, and curvature after neighborhood search, enabling point, line, and plane feature classification.The features are denoted σ1D, σ2D, and σ3D.
2) Multi-metric ICP Factors:
The LiDAR matching stage transforms extracted features using IMU motion prediction, finds map correspondences with a dynamic octree, and jointly minimizes point, line, and plane distance errors. Correspondence quality weights address noisy geometric data in airborne obscurants.
- IMU motion prediction transforms extracted features from the body frame to the world frame before map correspondence search.A dynamic octree finds point, line, and plane correspondences.
- The estimator jointly minimizes point-to-point, point-to-line, and point-to-plane distance metrics to estimate the optimal transformation.
- Point-to-line residuals use corresponding principal directions, while point-to-plane residuals use normal directions.
- Airborne obscurants can produce unreliable feature associations, so correspondence quality is weighted by how neighbor points fit point, line, or plane distributions.
- The correspondence formulation uses curvature thresholds, neighbor-point statistics, maximum distance errors, and neighbor counts.
3) LiDAR-inertial Odometry Optimization:
The LiDAR-inertial odometry optimization uses motion prediction, feature correspondences, weighted constraints, and a dynamic octree for efficient scan-map matching. Constraint dominance adapts to environmental degradation.
- The optimization minimizes a transformation objective using the Levenberg–Marquardt method.
- IMU odometry supplies the predicted pose prior and preintegration factor, while W_l and W_imu weight LiDAR and IMU correspondences.
- In geometrically degraded environments, unreliable LiDAR constraints are rejected or downweighted, while the IMU prior dominates optimization.
- Dynamic Octree: The dynamic octree stores voxel-indexed point data in a hash table and updates only the specific octree receiving new points.
1) Visual-inertial Odometry Optimization:
The visual-inertial odometry combines visual, inertial, marginalization, and IMU-derived pose-prior factors. It is evaluated on multi-sensor datasets covering visually and geometrically degraded environments.
- Each keyframe optimization combines visual reprojection, IMU preintegration, marginalization, and an IMU-odometry pose-prior factor.
- When vision is degraded, the IMU-derived prior dominates and unreliable vision factors are rejected; under good lighting, visual factors dominate.
- Dataset: The test platform uses a Velodyne VLP-16 LiDAR, Xsens IMU, fisheye camera, and Intel NUC onboard computer.
- Dataset: The datasets include dark-room, white-wall, constrained-environment, long-corridor, dust, and Urban Challenge Alpha Course sequences.
B. Robustness and Accuracy Evaluation
The evaluation compares Super Odometry with LiDAR- and vision-based odometry across visually, geometrically, and jointly degraded environments. Super Odometry achieves better accuracy in all evaluated cases.
- The evaluation compares LOAM, LIO-SAM, VINS, and Depth-enhanced VINS using translational Absolute Trajectory Error against ground truth trajectories.
- Super Odometry has better accuracy than the compared methods in visually, geometrically, and jointly degraded environments.
1) Robustness Comparison in Visually Degraded Environments:
Super Odometry is evaluated in dark, geometrically repetitive, and dusty environments where individual sensing modalities become unreliable. It reports low trajectory error and improved mapping relative to comparison methods.
- Visually Degraded Environments: 0.174m is Super Odometry’s lowest ATE on the dark-room sequence, outperforming the compared methods in poor lighting and aggressive motion.
- Geometrically Degraded Environments: 0.055m is Super Odometry’s lowest ATE in the reported geometrically degraded evaluation.
- Both Visually and Geometrically Degraded Environments: Super Odometry produces an accurate, well-aligned dust-sequence map, while LOAM and LIO-SAM produce maps with significant misalignment.
- Both Visually and Geometrically Degraded Environments: PCA-based feature extraction and feature-quality evaluation allow low-quality features to be treated as noise and excluded from state estimation.
- Real-time Performance: The real-time evaluation benchmarks dynamic-octree versus KD-tree insertion and query times and measures processing time for each submodule.
1) Dynamic Octree
Super Odometry uses a dynamic octree to improve real-time scan matching and supports high-frequency, asynchronous sensor fusion. Its LiDAR processing is faster than LIO-SAM on the Dust dataset, while the framework remains robust in challenging environments.
- Dynamic Octree: Dynamic octree tree-building time remains almost constant as frames increase, unlike KD-tree runtime, because only relevant subtrees are updated.Query times remain similar; hash-map lookups avoid rebuilding the entire tree.
- Runtime Performance: The framework processes sensor-fusion submodules in parallel and asynchronously rather than sequentially.Each optimizer caches constraints for processing.
- Runtime Performance: 21 ms versus 102 ms: Super Odometry’s average LiDAR processing time is lower than LIO-SAM’s on the Dust dataset.The paper attributes this to rejecting unreliable constraints and leveraging faster dynamic-octree insertion and querying.
- Runtime Performance: IMU Odometry directly provides state estimates at 1000 Hz.
- System Outcome: The framework is reported to deliver high-precision, resilient motion estimation and robustness to individual sensor failures in visually and geometrically degraded environments.