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. Note to Practitioners —Pose estimation aims to obtain ground vehicle’s position and heading in a certain coordinate system. The multisensor fusion has been a popular solution in reducing estimation errors, and constraints are helpful in improving the pose estimation performance by limiting possible solutions. This article proposes a framework to model multisensor pose estimation problems with constraints. The approximated estimation scheme inspired by the Monte Carlo method has been provided such that the framework can be applied to various systems without analytic modeling. Experiments in KITTI and self-collected sequences have been conducted to demonstrate the superiority of the proposed approach by fusing stereo visual odometry (SVO) and heading measurements under road constraints. The proposed approach can be ameliorated by adding an absolute reference in translation to correct the inherent translational drift in VO.

中文翻译：

---

地面车辆的道路约束几何姿势估计

具有状态或测量约束的姿势估计在自动驾驶汽车导航中很常见。为了固有地合并约束，本文提出了一种基于动态势场（DPF）的表示形式，以表示状态，测量值和连接的黎曼流形上的约束。状态方程和输出方程以DPF形式导出，这暗示着状态和测量的概率推断。通过向状态空间和测量空间中的约束子集投影点来合并约束，在约束空间中创建表示约束的DPF。已经为DPF设计了一种信息融合方案，该方案是从多传感器测量和约束条件获得的。KITTI和自我收集的序列已用于实验中，执业者注意 -姿态估算旨在获取地面车辆在特定坐标系中的位置和航向。多传感器融合一直是减少估计误差的流行解决方案，并且约束条件通过限制可能的解决方案有助于改善姿态估计性能。本文提出了一个框架，用于对具有约束的多传感器姿态估计问题进行建模。已经提供了由蒙特卡洛方法启发的近似估计方案，使得该框架无需分析建模即可应用于各种系统。通过在道路约束下融合立体视觉里程表（SVO）和航向测量，已经进行了KITTI和自我收集序列的实验，以证明所提出方法的优越性。