A Tight Filtering and Smoothing Fusion Method With Feature Tracking for LiDAR Odometry

Abstract

LiDAR odometry (LO) has gained popularity in recent years because of its growing applications. Existing solutions include filtering and smoothing, where the former optimizes the latest pose with high efficiency and the latter optimizes multiple poses with strong consistency. Still, it is promising to fuse filtering and smoothing in LO. In this paper, we propose a novel tight filtering and smoothing fusion method. Firstly, two nested sliding windows are maintained and they share estimated poses and features to balance the accuracy and efficiency. The large outer window is used to build a large local map for high-quality scan-to-map matching in the filtering module, and multiple poses in the small inner window are optimized in the smoothing module to improve local consistency and efficiency. Furthermore, sparse directed geometry points (DGPs) are tracked across window keyframes to produce feature-wise multiple residuals for poses optimization in both filtering and smoothing modules. Besides, the strategies of ratio-based refined pose optimization and efficient DGP feature triangulation are developed for pose refinement and local map update, respectively. The experiments on KITTI and Newer College (NC) datasets demonstrate the effectiveness of the proposed method.