Road-Constrained Geometric Pose Estimation for Ground Vehicles

Pose estimation with state or measurement constraints has been frequent in autonomous vehicle navigation. Aiming at incorporating constraints inherently, this article proposes a dynamic potential field (DPF)-based formulation to represent states, measurements, and constraints on connected Riemannian manifolds. The state equation and the output equation are derived in the DPF forms, which imply the probabilistic inference with states and measurements. Constraints are incorporated by projecting points toward a constraint subset in the state space and the measurement space, where the DPF representing constraints is created. An information fusion scheme has been designed for DPFs, which are obtained from multisensor measurements and constraints. KITTI and self-collected sequences have been used in experiments, during which it is observed that the rotational drift is corrected and translational errors are reduced, thanks to the fusion of stereo visual odometry (SVO), heading measurements, and road maps.