Incremental Real-Time Multibody VSLAM with Trajectory Optimization Using Stereo Camera

TL;DR

This paper introduces a real-time, incremental VSLAM method that jointly estimates camera and moving object trajectories using stereo camera data and trajectory optimization, improving outdoor navigation in dynamic environments.

Contribution

It presents a novel joint inference approach for camera and object trajectories with real-time nonlinear optimization using stereo scene flow segmentation.

Findings

Outperforms previous methods on KITTI dataset

Accurately estimates camera and object trajectories simultaneously

Operates in real time in dynamic outdoor scenes

Abstract

Real time outdoor navigation in highly dynamic environments is an crucial problem. The recent literature on real time static SLAM don't scale up to dynamic outdoor environments. Most of these methods assume moving objects as outliers or discard the information provided by them. We propose an algorithm to jointly infer the camera trajectory and the moving object trajectory simultaneously. In this paper, we perform a sparse scene flow based motion segmentation using a stereo camera. The segmented objects motion models are used for accurate localization of the camera trajectory as well as the moving objects. We exploit the relationship between moving objects for improving the accuracy of the poses. We formulate the poses as a factor graph incorporating all the constraints. We achieve exact incremental solution by solving a full nonlinear optimization problem in real time. The evaluation is performed on the challenging KITTI dataset with multiple moving cars.Our method outperforms the previous baselines in outdoor navigation.