Three-Stage Hierarchical LIDAR SLAM with Confidence-Based Pose Selection for Enhanced Vertical Accuracy

Multi-line LiDAR Simultaneous Localization and Mapping (SLAM) is a fundamental technology for autonomous driving, robotic navigation, and environmental perception. Achieving precise localization in GPS-denied environments is crucial for advanced autonomous systems, particularly in complex and dynamic scenarios. This paper presents a hierarchical threestage optimization framework designed to significantly enhance SLAM accuracy, especially in challenging environments. The proposed method optimizes feature point clouds across three distinct clusters: the previous frame, the global point cloud, and keyframe clusters. Each stage of optimization progressively refines the pose estimation with increasing precision and stability, allowing the system to adapt to various environmental conditions. The first stage provides initial values for subsequent stages, while the second and third stages yield results with varying degrees of accuracy and stability. To achieve an optimal balance between precision and robustness, we introduce a confidence-based result selection mechanism, which evaluates the reliability of the thirdstage results to determine the final pose estimate. Extensive experiments conducted on the KITTI public dataset and the NTU VIRAL UAV dataset demonstrate that our approach achieves substantial improvements in SLAM performance, with notable gains in vertical localization accuracy and overall robustness.