Tightly-coupled Lidar-GNSS-Inertial Fusion Odometry and Mapping

This paper proposes a tightly-coupled lidar-GNSS-inertial fusion system that achieves accurate state estimation and mapping for robot navigation. The system effectively fuses lidar points, GNSS measurements, and IMU data using the iterated error state Kalman filter algorithm to obtain precise, drift-free, and real-time odometry. To optimize computational efficiency, an incremental tree structure, ikdtree, is employed for managing a local map, and different Kalman gain formulas are utilized to process GNSS and lidar point observations separately to lower the computation load. Furthermore, a factor graph is introduced to optimize the pose further. By selectively introducing keyframes based on the odometry estimation, the graph incorporates odometry, GNSS measurements, and loop closure constraints to optimize all keyframe poses, resulting in a precise trajectory and global map. Finally, extensive experiments are conducted using the KITTI dataset and several real-world scenarios to validate the proposed approach. The experimental results demonstrate that our method achieves precise localization and mapping across diverse environments.