Hierarchical Estimation-Based LiDAR Odometry With Scan-to-Map Matching and Fixed-Lag Smoothing

Abstract

LiDAR odometry (LO) has gained popularity in recent years due to accurate depth measurement and robustness to illumination. However, existing solutions to LO mainly rely on scan-to-map matching in which only current pose is optimized. To further reduce the accumulated error of pose estimation, the fixed-lag smoothing that optimizes fixed-size poses simultaneously by matching corresponding point features of multiple frames in visual odometry provides an important reference. The integration of fixed-lag smoothing with LO still needs further exploration. In this paper, a general fixed-lag smoothing module is proposed, which can be appended to existing LO framework to improve the consistency of trajectory. Also, a fast scan-to-map matching module based on sparse features and efficient feature management is developed to guarantee the real-time performance. Besides, the feature-centric feature management strategy is adopted in both scan-to-map matching and fixed-lag smoothing modules, which makes the proposed LO more efficient. On this basis, a hierarchical estimation-based LiDAR odometry is presented, where low-level scan-to-map matching estimates pose of each frame by aligning associated features in the frame and corresponding surrounding map with high efficiency, and high-level fixed-lag smoothing further optimizes keyframe poses in a sliding window by matching associated features among multiple frames with high accuracy. As a result, a fast and accurate pose estimation is achieved, which is verified by experiments on the KITTI dataset, Newer College dataset, and an actual outdoor scenario.