Tightly-Coupled LiDAR-Visual-Inertial SLAM and Large-Scale Volumetric Occupancy Mapping

TL;DR

The paper tackles the challenge of robust autonomous navigation by delivering a fully tightly-coupled LiDAR-Visual-Inertial SLAM and volumetric occupancy mapping system that uses local submaps to scale to large environments. It introduces a novel, correspondence-free LiDAR residual formulation that operates directly on occupancy values $L(\cdot)$ and gradients $\nabla L(\cdot)$, enabling frame-to-map and map-to-map factors within a unified factor-graph optimization built on top of a VI-SLAM backbone. Key contributions include the occupancy-based residuals, submap-based global alignment, and extensive validation on the HILTI SLAM Challenge showing state-of-the-art localisation and globally consistent occupancy submaps suitable for navigation. The approach demonstrates strong generalization across LiDAR sensors and real-world scenarios, providing a practical pipeline for navigation and exploration using consistent 3D maps.

Abstract

Autonomous navigation is one of the key requirements for every potential application of mobile robots in the real-world. Besides high-accuracy state estimation, a suitable and globally consistent representation of the 3D environment is indispensable. We present a fully tightly-coupled LiDAR-Visual-Inertial SLAM system and 3D mapping framework applying local submapping strategies to achieve scalability to large-scale environments. A novel and correspondence-free, inherently probabilistic, formulation of LiDAR residuals is introduced, expressed only in terms of the occupancy fields and its respective gradients. These residuals can be added to a factor graph optimisation problem, either as frame-to-map factors for the live estimates or as map-to-map factors aligning the submaps with respect to one another. Experimental validation demonstrates that the approach achieves state-of-the-art pose accuracy and furthermore produces globally consistent volumetric occupancy submaps which can be directly used in downstream tasks such as navigation or exploration.

Figures (5)

• Figure 1: Horizontal slices of the occupancy fields and 3D reconstructions from Sequence Exp21 of the HILTI SLAM Challenge Hilti22. All submaps are overlayed. Meshes of the surfaces (brown) are extracted from the occupancy field as zero-crossings. The blue area denotes free space extracted for gravity-aligned slices through the submaps. The estimated trajectory is shown in green.
• Figure 2: High-level structure of the proposed tightly-coupled LVI-SLAM and submapping framework. Motion compensation for LiDAR point clouds is performed to formulate live frame-to-map factors and to update the current active submap. Upon submap completion, the most overlapping previous submap is determined and map-to-map factors are fused into the optimisation problem.
• Figure 3: Inverse sensor model used in Supereight2 SE2 for two measurements (1m, 5m). The log-odds occupancy probability along a ray measurement is expressed as a function of the difference $d_r$ between query points along the ray and the measured distance $z_r$. The occupancy values are clipped in front of the surface at a minimum $L_{\mathrm{min}}$ reached at $3 \sigma$ where $\sigma$ is a distance dependent uncertainty value. It grows linearly up to half the surface thickness $\tau(z_r)$. For more details, see SE2.
• Figure 4: Optimisation Factor Graph of the LiDAR-Visual-Inertial Estimator. Left: The real-time estimator connects set of current keyframe states and non-keyframe states by IMU errors and visual reprojection errors. It also shows the active submap keyframe and the last completed submap keyframe. For every state in the optimisation window, we can formulate live LiDAR factors between every live frame and the last completed submap. Right: OKVIS2 connects keyframe poses through relative pose errors at a later stage. In addition to the live LiDAR factors, measurements between frames can be aggregated and map-to-map LiDAR factors can be added to the factor graph. Every map will be connected to the previous submap and optionally an older submap if the geometric overlap surpasses a threshold.
• Figure 5: Outdoor Example: Reconstruction (brown), horizontal slice of the free space (blue) and estimated trajectory (green).