STELVIO: Exploring Factor Graphs for a Robust Stereo-Visual-LiDAR-Inertial Odometry

Abstract. Accurate and robust odometry is critical for mobile mapping and autonomous navigation, particularly in complex environments where single-sensor approaches struggle. While LiDAR and visual odometry each provide valuable motion estimation, they are susceptible to failures in conditions unfavorable for the specific odometry type. Fusing multiple modalities enhances robustness, yet effective integration remains challenging due to differences in heterogenous sensor data representation. This study presents STELVIO, a flexible factor graph-based framework for Stereo LiDAR-Visual-Inertial Odometry. By combining LiDAR-inertial odometry with stereo visual odometry, STELVIO improves trajectory estimation by leveraging the strengths of each modality. The system introduces adaptive fusion strategies, ranging from loose, odometry-only pose graph coupling to an extensive factor graph approach, utilizing visual features and LiDAR-derived range factors. This modular structure allows for balancing computational efficiency with robustness, making it suitable for real-time applications and accuracy-oriented mapping applications. Evaluation is conducted using an in-house mobile mapping system in a challenging indoor environment. Initial results highlight the effectiveness of the fusion approach in reducing drift and improving localization consistency compared to single-sensor methods. The findings demonstrate the potential of multi-sensor integration for robust and scalable mobile mapping solutions.