Abstract
We propose a framework for tightly-coupled lidar-visual-inertial odometry via smoothing and mapping, LVI-SAM, that achieves real-time state estimation and map-building with high accuracy and robustness. LVI-SAM is built atop a factor graph and is composed of two sub-systems: a visual-inertial system (VIS) and a lidar-inertial system (LIS). The two sub-systems are designed in a tightly-coupled manner, in which the VIS leverages LIS estimation to facilitate initialization. The accuracy of the VIS is improved by extracting depth information for visual features using lidar measurements. In turn, the LIS utilizes VIS estimation for initial guesses to support scan-matching. Loop closures are first identified by the VIS and further refined by the LIS. LVI-SAM can also function when one of the two sub-systems fails, which increases its robustness in both texture-less and feature-less environments.
Introduction
Autonomous robotic navigation relies heavily on Simultaneous Localization and Mapping (SLAM). However, individual sensors often face specific failure modes. Lidar sensors provide high-precision geometric data but struggle in "structure-less" environments like open fields or smooth corridors. Conversely, cameras excel in texture-rich areas but are sensitive to lighting changes and rapid motion. LVI-SAM addresses these challenges by tightly coupling Lidar, Visual, and Inertial sensors within a unified factor graph framework, ensuring resilience even when specific sensors are degraded.
System Architecture
LVI-SAM is composed of two primary sub-systems—a Visual-Inertial System (VIS) and a Lidar-Inertial System (LIS)—that interact bidirectionally.
The VIS handles camera and IMU data, performing feature tracking while utilizing the LIS to provide accurate depth for visual features. By projecting Lidar points onto the camera frame, the system bypasses the traditional difficulty of monocular depth estimation. Simultaneously, the LIS uses the VIS's state estimate as an initial guess for scan-matching, providing a "warm start" that prevents failure during aggressive maneuvers.
Factor Graph Optimization
The core of LVI-SAM is a factor graph optimized using iSAM2 (Incremental Smoothing and Mapping). This graph integrates multiple constraints: IMU preintegration, visual odometry, lidar odometry, and loop closure.
The system includes robust failure detection. If one sub-system fails—for instance, the VIS in a dark environment—the LIS maintains localization and assists in re-initializing the VIS once conditions improve. Loop closures are identified by the VIS using a "Bag of Words" approach and subsequently refined by the LIS using geometric constraints, combining the strengths of both modalities.
Experimental Evaluation
The framework was evaluated across diverse environments:
- Urban: A campus setting with buildings and vegetation, testing performance in GPS-denied areas.
- UGV (Jackal): Benchmarked against state-of-the-art methods in feature-rich environments.
- Handheld: A challenging open-field test designed to trigger Lidar degeneracy.
In the Jackal dataset, LVI-SAM achieved a Root Mean Square Error (RMSE) of 0.67m, significantly outperforming LIO-SAM (1.52m) and VINS-Mono (4.49m). Notably, in the handheld open-field test, Lidar-only methods failed entirely due to lack of geometric structure, whereas LVI-SAM maintained a successful trajectory with an RMSE of 0.83m.
Conclusion
LVI-SAM demonstrates that tight coupling of lidar and visual sensors significantly enhances SLAM robustness and accuracy. By cross-referencing data—using Lidar for depth and Vision for initial guesses—the system overcomes the inherent limitations of single-sensor modalities. Future work will explore the integration of additional sensors like radar and automated extrinsic calibration.
Summary updated