TRLO: An Efficient LiDAR Odometry with 3D Dynamic Object Tracking and Removal

TL;DR

TRLO is proposed, a dynamic LiDAR odometry that efficiently improves the accuracy of state estimation and generates a cleaner point cloud map and a novel hash-based keyframe database management is proposed for fast access to search keyframes.

Abstract

Simultaneous state estimation and mapping is an essential capability for mobile robots working in dynamic urban environment. The majority of existing SLAM solutions heavily rely on a primarily static assumption. However, due to the presence of moving vehicles and pedestrians, this assumption does not always hold, leading to localization accuracy decreased and maps distorted. To address this challenge, we propose TRLO, a dynamic LiDAR odometry that efficiently improves the accuracy of state estimation and generates a cleaner point cloud map. To efficiently detect dynamic objects in the surrounding environment, a deep learning-based method is applied, generating detection bounding boxes. We then design a 3D multi-object tracker based on Unscented Kalman Filter (UKF) and nearest neighbor (NN) strategy to reliably identify and remove dynamic objects. Subsequently, a fast two-stage iterative nearest point solver is employed to solve the state estimation using cleaned static point cloud. Note that a novel hash-based keyframe database management is proposed for fast access to search keyframes. Furthermore, all the detected object bounding boxes are leveraged to impose posture consistency constraint to further refine the final state estimation. Extensive evaluations and ablation studies conducted on the KITTI and UrbanLoco datasets demonstrate that our approach not only achieves more accurate state estimation but also generates cleaner maps, compared with baselines.

Figures (5)

• Figure 1: Our global clean map with dynamic objects removal on Urbanloco dataset. A and B are two examples of local map details at crowded traffic intersections, presenting that the superiority of our method on mapping.
• Figure 2: Our system architecture. The pillars-based 3D object detector first is used for preprocessing of the raw point cloud to detect dynamic and semi-static objects, resulting in the generation of a 3D object bounding boxes. Subsequently, 3D multi-object tracker is applied to identify and remove dynamic objects. The adjacent static scans $P_k$ are input to calculate the scan-to-scan(S2S) transformation. The initial value is propagated to the world frame and used for the secondary Fast GICP of scan-to-map(S2M). $P_k$ is scan-matched with the submap $S_k$ composed of selective keyframes. Finally, the S2M transformation is further optimized with the posture consistency constraint imposed by the detected bounding boxes to obtain a refined global robot's pose, which is checked against multiple metrics to determine if it should be stored in keyframe hash dataset.
• Figure 3: Effect of the proposed detection and filtration of dynamics. (a) Raw point cloud. (b) 3D bounding boxes of dynamic and semi-static objects detected by our 3D object detector (blue boxes). (c) 3D bounding boxes of dynamic objects identified by our 3D multi-object tracker (green boxes). (d) Static point cloud after removing the dynamic objects, which will be input for LiDAR Odometry.
• Figure 4: Mapping results. (a) Global map generated by traditional method. (b) Local details of the global map generated by traditional method, where red points represent ghosttail caused by moving objects. (c) Global map generated by our method. (d) local details of the global map generated by our method. It is clear that our method effectively detect and filter moving objects, resulting in more consistent results.
• Figure 5: z-axis drift of all methods on ablation study. With imposing the bounding box consistency constraint, the z-axis drift is inhibited and the odometry accuracy is improved.