Tightly Coupled LiDAR-Inertial Odometry With Geometric-Uncertainty Modeling

Uncertainty awareness in geometric observations is crucial for LiDAR-based simultaneous localization and mapping (SLAM) since it determines the direction of optimization and ultimately affects the accuracy of localization. In this paper, we present a tightly-coupled LiDAR-inertial odometry system with geometric uncertainty modeling framework for accurate and robust state estimation. The proposed LIO system is designed by an Iterative Error State Kalman Filter (IESKF) to effectively fuse LiDAR and IMU measurement, and employs the voxel structure to query and update LiDAR features. To observe the reliability of geometric measurements, we propose novel uncertainty models of LiDAR point, line and plane features, and apply them to construct point-to-line and point-to-plane constraints. In the maximization of posterior state estimation, we propagate the uncertainties of geometric constraints to the iterative error state Kalman filter, further consider the weights of geometric constraints, and enrich the comprehensiveness of the geometric residuals. To reduce the memory consumption of the system, we propose a cumulative update method with uncertainty parameters to avoid repeated parameter computations. We extensively evaluate our method in diverse public datasets. The results validate the high precision and robustness of the proposed method when compared to other state-of-the-art methods in challenge scenes with high perceptual uncertainties.