Visual-Odometric Localization and Mapping for Ground Vehicles Using SE(2)-XYZ Constraints

This paper focuses on the localization and mapping problem on ground vehicles using odometric and monocular visual sensors. To improve the accuracy of vision based estimation on ground vehicles, researchers have exploited the constraint of approximately planar motion, and usually implemented it as a stochastic constraint on an SE(3) pose. In this paper, we propose a simpler algorithm that directly parameterizes the ground vehicle poses on SE(2). The out-of SE(2) motion perturbations are not neglected, but incorporated into an integrated noise term of a novel SE(2)-XYZ constraint, which associates an SE(2) pose and a 3D landmark via the image feature measurement. For odometric measurement processing, we also propose an efficient preintegration algorithm on SE(2). Utilizing these constraints, a complete visual-odometric localization and mapping system is developed, in a commonly used graph optimization structure. Its superior performance in accuracy and robustness is validated by real-world experiments in industrial indoor environments.