APMC-LOM: Accurate 3D LiDAR Odometry and Mapping Based on Pyramid Warm-Up Registration and Multi-Constraint Optimization

Simultaneous localization and mapping (SLAM) based on LiDAR plays a pivotal role in many unmanned systems, but currently suffers from drift in trajectory estimation and lacks of robustness, resulting in inconsistent global maps. This paper proposes an accurate and robust LiDAR SLAM system to achieve low-drift ego-motion estimation and globally consistent mapping for unmanned ground vehicles (UGVs) in diverse environments. Firstly, a pyramid warm-up registration method is proposed to directly match the current scan with the map without feature extraction. More importantly, it utilizes the original geometric information to improve the registration accuracy and adopts a fast covariance matrix calculation method to greatly enhance the registration speed. Secondly, a submap generation method is proposed by formulating an anti-slip strategy and a point cloud similarity metric. It effectively prevents the loss of critical information while establishing strong constraints between keyframes and the map. Finally, a local-to-global optimization factor graph is constructed by establishing multi-level constraint relationships to optimize the overall system accuracy. The proposed method is compared with the current state-of-the-art LiDAR SLAM methods on several challenging datasets, including the KITTI, NeBula, and Newer College datasets. Experimental results show that our method has higher trajectory estimation accuracy and map consistency, and performs robustly in disparate environments.