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

TL;DR

DL-SLOT is proposed, a dynamic LiDAR SLAM and object tracking method that integrates the state estimations of both the autonomous vehicle and the stationary and dynamic objects in the environment into a unified optimization framework, and achieves better accuracy than SLAMand object tracking baseline methods.

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.

Figures (10)

• Figure 1: Mapping result of KITTI Odometry dataset 05 sequence. The gray shading indicates the height information of the environment. The green and red dots indicate the ego-trajectory and the trajectories of dynamic objects, respectively.
• Figure 2: Pose notations of autonomous vehicle and objects. $\{w\}$ is the world coordinate system and $\{l\}$ is the self-vehicle coordinate system. $X$ is the autonomous vehicle's pose, and $T_{t-1}^{t}$ is the odometry between the adjacent frames. $_{w}B_{t}^{i}$ and $_{l}B_{t}^{i}$ are the object's poses in different coordinate systems.
• Figure 3: The system architecture of DL-SLOT. The system comprises SLOT front-end, trajectory-based data association, and SLOT back-end.
• Figure 4: Schematic of trajectory association. The colored circle and triangle denote two different objects. The solid green line represents the approximated object trajectory. The dashed circle and triangle represent the detected objects, and the dashed rectangular indicates a location that is prone to an incorrect association. The yellow dashed line represents the erroneous prediction result.
• Figure 5: Local optimization framework. The red node represents the ego-pose, and the yellow node represents the detected object pose, and the blue node represents the motion of associated object. The line connecting the nodes is the constructed constraint.