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.
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 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). The current static scan is scan-matched with the submap 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.
Compared with traditional LO systems, our approach generally achieves encouraging performance through the proposed dynamic removal and bounding box consistency constraint strategies, especially for the highly dynamic sequences. Additionally, the proposed method is superior to LIO-SEGMOT and LIMOT (dynamic LO), apart from that the APE of Urbanloco03 sequence is lower than that of LIMOT. Notably, our approach can obtain competitive results using only LiDAR scans as input, which demonstrates the effectiveness of dynamic removal and robust registration mechanism.
In the map generated by DLO, we clearly see the severe ghosttail in the map caused by moving cars. Comparatively, our method adeptly identifies and filters moving objects while retaining valuable semi-static objects. Compared with LIO-SEGMOT, LIMOT and RF-A-LOAM, our method shows better dynamic removal performance. Ultimately, we obtain a high-precision LiDAR odometry and build a cleaner map.
The PR and RR in the Table represent the preserved rate of the static points and the removed rate of dynamic points. From the statistic, the PR scores of our method are higher than other baselines, while the RR scores are suboptimal to LIMOT. Balancing PR and RR, the F1-Score of our method is competitive.
Tracker: LO system with UKF-based object tracker outperforms EKF-based, benefiting from the more robust dynamic tracking. Theoretically, EKF addresses the nonlinear state estimation by using linear approximation, which may introduce tracking bias.
Dynamic Removal: The results demonstrate that the outliers in the environment are effectively filtered by adding our dynamic removal step, resulting in a more accurate odometry estimation. This improvement is particularly obvious when dealing with the highly dynamic UrbanLoco dataset.
Bounding Box Consistency Constraint: The z-axis drift of the odometry is effectively inhibited by imposing the bounding box consistency constraint. Combined with dynamic removal and bounding box consistency constraint, our complete system achieves more accurate results. The translation accuracy of KITTI07 dataset and UrbanLoco05 dataset increases by 39.3% and 30.4%, respectively
TRLO