A stereo vision SLAM with moving vehicles tracking in outdoor environment

The assumption of a static environment is a prerequisite for most of the traditional visual simultaneous localization and mapping (v-SLAM) algorithms, which limits their widespread application in a dynamic environment. Furthermore, in many applications such as autonomous driving, robot collaboration and AR/VR, it is necessary to track the moving objects in the environment. In this work, we propose a v-SLAM method that can effectively track multiple objects in dynamic environments by integrating a 3D object detection thread into the ORB-SLAM2 framework. The dynamic objects were detected and tracked in three steps. Firstly, 3D object detection was performed on the current frame, and the 3D bounding box was projected into a bird's-eye view. Secondly, an association for the object is made based on the motion state of the object and the bounding box in the bird’s-eye view. Thirdly, we track the object and remove feature points corresponding to the dynamic region. In addition, we set up a multi-view constraint adjustment for static objects to jointly optimize the pose of the camera, object, and map point. Experiments have been conducted on the KITTI-odom and KITTI-raw datasets. The performance of our method was verified in challenging scenarios. We demonstrate that dynamic object tracking not only provides useful information for scene understanding, but also help to improve camera tracking.