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

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.

I. Introduction

Lidar odometry (LO) is an important direction, which outputs the pose estimation with only 3D LiDAR (Light Detection and Ranging) sensor measurement as input. It is a core task in many applications, such as autonomous car driving, power plant inspection, etc. LO has drawn attention in recent years with the falling price and increasing popularity of 3D LiDAR sensors [1]–[3]. Compared with the visual sensor, the LiDAR sensor possesses accurate range measurements with robustness to the light, which makes it suitable for 24-hour applications.