Fast Binocular SLAM Algorithm Combining the Direct Method and the Feature-based Method

To obtain the accurate 3D poses of the robot with a binocular camera and the surroundings information in real time, a binocular SLAM (simultaneous localization and mapping) algorithm combining the direct method and the feature-based method is proposed. The proposed algorithm comprises four threads:tracking thread, feature extraction thread, local mapping thread and loop closing thread. In the tracking thread, the initial pose estimation of the binocular camera and the feature alignment are obtained by minimizing image photometric errors. A more accurate pose estimation is obtained by minimizing reprojection errors of local map points. In the feature extraction thread, the keypoints and the descriptors are extracted to guarantee that the subsequent local mapping thread runs smoothly when processing more keyframes. In the local mapping thread, the local map is settled, and the local BA (bundle adjustment) is implemented for the optimization of the local keyframe poses and the local map point locations to improve the local consistency of SLAM. In the loop closing thread, the loop detection and the loop optimization for keyframes are executed, to enhance the global consistency of SLAM. Besides, the localization problem is settled for a kidnaped robot returning to its previous detected environment in the loop closing thread. The experiments on KITTI datasets, TUM datasets and the collected binocular data show that, the proposed algorithm ensures the precise localization and the output of camera pose at a higher frame rate comparing with ORB-SLAM2 algorithm, and also brings more information of the poses and environment when the robot is kidnaped.