The open source code of Visual Localization and Mapping Leveraging the Constraints of Local Ground Manifolds. This work is published on IEEE Robotics and Automation Letters with ICRA2022 Oral Report.

In order to improve the accuracy of simultaneous localization and mapping problem, plane motion assumption is often used for advanced ground vehicle SLAM system. However, such an assumption is not always suitable to complex and changeable road scenes. In this letter, we propose a stereo-vision based SLAM framework that tightly couples the local ground manifold constraints into accurate camera trajectory estimation. Instead of considering a planar manifold assumption, we model the road as a sequence of local planes with different slopes named local ground manifolds (LGM). The impact region of the LGM is represented as a spherical area in the map, where the vehicle’s motion is constrained by the corresponding local plane model. ORB features and road segmentations are utilized to perform the environmental reconstruction and ground manifold representation. The structures of surroundings and the plane normal of LGMs are jointly optimized with the trajectory of the vehicle within a novel point-LGM tightly-coupled bundle adjustment framework. The experiments on KITTI datasets demonstrate that the proposed ground manifold representation can greatly benefit the camera trajectory estimation.

This project is implmented based on ORB_SLAM2. All the DEPENDENCIES are the same as ORB_SLAM2's.

This project uses kitti odometery datasets. The structure is, for example, in kitti/00:

You can use gray or color images to test the algorithm. The segmentation of the images is generated by a semantic segmentation algorithm Kittiseg. For testing, the road segmented images of 06 sequence are provided in onedrive.

./Examples/Stereo/stereo_kitti_seg Vocabulary/ORBvoc.txt Examples/Stereo/KITTI04-12.yaml KITTI/sequence/folder

If you refer to this code in an academic work, please cite:

@article{zhou2022visual,
  title={Visual Localization and Mapping Leveraging the Constraints of Local Ground Manifolds},
  author={Zhou, Pengkun and Liu, Yuzhen and Gu, Pengfei and Liu, Jiacheng and Meng, Ziyang},
  journal={IEEE Robotics and Automation Letters},
  volume={7},
  number={2},
  pages={4196--4203},
  year={2022},
  publisher={IEEE}
}