DL-SLOT: Dynamic LiDAR SLAM and object tracking based on collaborative graph optimization

Xuebo Tian, Zhongyang Zhu, Junqiao Zhao, Gengxuan Tian, Chen YearXiv 2022 · OpenReview

Abstract

Ego-pose estimation and dynamic object tracking are two critical problems for autonomous driving systems. The solutions to these problems are generally based on their respective assumptions, i.e., the static world assumption for simultaneous localization and mapping (SLAM) and the accurate ego-pose assumption for object tracking. However, these assumptions are challenging to hold in dynamic road scenarios, where SLAM and object tracking become closely correlated. Therefore, we propose DL-SLOT, a dynamic LiDAR SLAM and object tracking method, to simultaneously address these two coupled problems. This method integrates the state estimations of both the autonomous vehicle and the stationary and dynamic objects in the environment into a unified optimization framework. First, we used object detection to identify all points belonging to potentially dynamic objects. Subsequently, a LiDAR odometry was conducted using the filtered point cloud. Simultaneously, we proposed a sliding window-based object association method that accurately associates objects according to the historical trajectories of tracked objects. The ego-states and those of the stationary and dynamic objects are integrated into the sliding window-based collaborative graph optimization. The stationary objects are subsequently restored from the potentially dynamic object set. Finally, a global pose-graph is implemented to eliminate the accumulated error. Experiments on KITTI datasets demonstrate that our method achieves better accuracy than SLAM and object tracking baseline methods. This confirms that solving SLAM and object tracking simultaneously is mutually advantageous, dramatically improving the robustness and accuracy of SLAM and object tracking in dynamic road scenarios.

Introduction

Most Lidar SLAM systems rely on a "static world assumption," assuming the environment remains stationary. Conversely, object tracking systems usually assume the vehicle’s own pose (ego-pose) is perfectly known. In real-world dynamic road scenarios, both assumptions often fail. Dynamic objects like cars and cyclists create noise for SLAM, leading to drift, while inaccurate ego-pose estimates hinder a tracker's ability to predict object trajectories.

DL-SLOT breaks this cycle by integrating Lidar SLAM and multi-object tracking into a single, unified graph optimization framework. By treating both the vehicle and surrounding objects as interconnected nodes, the system achieves significantly higher localization accuracy in chaotic, high-traffic environments compared to traditional independent methods.

System Architecture

The DL-SLOT pipeline consists of four main stages designed to handle the coupling between localization and tracking:

  1. Preprocessing & Filtering: A 3D object detector identifies potential dynamic objects. Points belonging to these objects are temporarily filtered to provide a "clean" point cloud for initial odometry.
  2. Lidar Odometry: A front-end calculates the initial ego-motion using the filtered static point cloud.
  3. Time-Series Data Association: The system maintains a sliding window of history, using a constant velocity model and the Hungarian algorithm to match detections with existing trajectories.
  4. Unified Graph Optimization: The core innovation where ego-states and object states are jointly refined.
Overview of the DL-SLOT system architecture showing the flow from preprocessing to optimization.

Collaborative Graph Optimization

The system builds a factor graph that serves as a collaborative optimization framework. In this graph, different nodes represent distinct states: red nodes represent the vehicle's ego-pose, yellow nodes represent object poses, and blue nodes represent the change in an object's pose. Constraints (edges) link these nodes based on Lidar odometry, observations, and a constant velocity motion model for dynamic objects.

The local optimization framework illustrating the factor graph with ego-pose, object pose, and pose change nodes.

By optimizing this entire graph simultaneously, the system refines the vehicle's path using the movement of objects as additional constraints. Once an object is tracked and confirmed as static, it is treated as a landmark, which significantly stabilizes the vehicle's pose estimation. This mutual refinement ensures that better ego-pose estimates lead to more precise tracking, and vice versa.

Experimental Results

The authors evaluated DL-SLOT using the KITTI tracking and odometry datasets. Quantitative results show a clear advantage for the unified approach:

  • Localization Accuracy: In the KITTI "tracking 07" sequence, DL-SLOT reduced the Relative Translation Error (RTE) from 10.47 (A-LOAM baseline) to 4.51.
  • Robustness: In high-speed highway scenes (e.g., "tracking 20"), the local optimization improved positioning accuracy by 1.28 meters over the baseline.
  • Real-time Efficiency: Despite the complexity of the graph, the trajectory association and optimization modules run at approximately 50 FPS (2.3ms and 13.5ms respectively), making the system suitable for real-time autonomous driving.

Conclusion

DL-SLOT demonstrates that solving SLAM and object tracking simultaneously is mutually beneficial. While the system currently relies on the accuracy of the initial 3D object detector and a simple constant velocity motion model, it provides a robust framework for navigation in dynamic environments. Future work will focus on integrating global loop closure and exploring deep-learning-based Lidar odometry to further enhance performance.

Summary updated

Abstract

Ego-pose estimation and dynamic object tracking are two critical problems for autonomous driving systems. The solutions to these problems are generally based on their respective assumptions, \ie{the static world assumption for simultaneous localization and mapping (SLAM) and the accurate ego-pose assumption for object tracking}. However, these assumptions are challenging to hold in dynamic road scenarios, where SLAM and object tracking become closely correlated. Therefore, we propose DL-SLOT, a dynamic LiDAR SLAM and object tracking method, to simultaneously address these two coupled problems. This method integrates the state estimations of both the autonomous vehicle and the stationary and dynamic objects in the environment into a unified optimization framework. First, we used object detection to identify all points belonging to potentially dynamic objects. Subsequently, a LiDAR odometry was conducted using the filtered point cloud. Simultaneously, we proposed a sliding window-based object association method that accurately associates objects according to the historical trajectories of tracked objects. The ego-states and those of the stationary and dynamic objects are integrated into the sliding window-based collaborative graph optimization. The stationary objects are subsequently restored from the potentially dynamic object set. Finally, a global pose-graph is implemented to eliminate the accumulated error. Experiments on KITTI datasets demonstrate that our method achieves better accuracy than SLAM and object tracking baseline methods. This confirms that solving SLAM and object tracking simultaneously is mutually advantageous, dramatically improving the robustness and accuracy of SLAM and object tracking in dynamic road scenarios.
BibTeX
@misc{tian2022dl,
  author = {Xuebo Tian and Zhongyang Zhu and Junqiao Zhao and Gengxuan Tian and Chen Ye},
  title = {{DL-SLOT: Dynamic LiDAR SLAM and object tracking based on collaborative graph optimization}},
  year = {2022},
  eprint = {2212.02077},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2212.02077},
}
Figure