Multi-object tracking and trajectory optimization based on improved data association and two-stage optimization in highly dynamic urban environments

Abstract It is essential for them to accurately and efficiently estimate both their own pose and the poses of surrounding dynamic objects in real time during autonomous exploration. Although most existing approaches achieve accurate self-localization or object detection and tracking individually, there is limited research on effectively integrating these two tasks, despite their inherently coupled and interactive nature. To address this challenge, this paper proposes a multi-object tracking and trajectory optimization framework based on enhanced data association and a two-stage optimization strategy, enabling robust localization of both the ego-agent and surrounding dynamic objects in highly dynamic urban environments. In the front-end, we incorporate an improved data association algorithm into a multi-object tracking pipeline and combine it with a GICP-based fast registration method to achieve efficient dynamic tracking. In the back-end, based on the front-end tracking results, we model the states of moving objects as dynamic tracking factors and integrate them into a global factor graph. This allows for joint optimization of the ego-pose and the poses of dynamic objects, resulting in high-precision multi-object tracking. Extensive comparative experiments on public high-dynamic datasets demonstrate the state-of-the-art performance and advantages of the proposed method in multi-object tracking and self-localization, where the accuracy of self-localization and tracking has increased by 19.7% and 3%, respectively, compared with the most advanced method.