FAST-LiDAR-SLAM: A Robust and Real-Time Factor Graph for Urban Scenarios With Unstable GPS Signals

This paper introduces FAST-LiDAR-SLAM: a robust, fast, and versatile LiDAR SLAM framework, which is specifically designed for urban scenarios with unstable GPS signals. The framework establishes a flexible and compact factor graph model that incorporates various relative and absolute measurements (including loop closure and GPS) as factors to optimize state estimation and dense mapping. Two key innovations are presented in this work. Firstly, it adopts a direct point cloud registration and map updating system in contrast to the traditional feature-based SLAM. In this way, the system can effectively exploit the fine structure of the environment and maintain robustness when confronted with non-structural road scenes. Secondly, an improved strong tracking extended Kalman filter (STEKF) is proposed to exclude anomalous GPS data in the back-end processing. Experiments show that FAST-LiDAR-SLAM achieves single-frame pose estimation in <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"> <tex-math notation="LaTeX">$20~ms$ </tex-math></inline-formula> for solid-state LiDAR tests, outperforming Faster-LIO in accuracy. For rotating LiDAR, FAST-LiDAR-SLAM achieves comparable accuracy to LIO-SAM while significantly improving computational efficiency.