Efficient and Robust Absolute Pose Estimation via Gravity-Prior-Driven Transformation Decoupling and Pose Refinement
Hu Cao, Qianyi Yang, Xinyi Li, Jiong Liu, Yinlong Liu, Alois Knoll
cs.CV
2026-09-01
用重力把PnP从6自由度收到4自由度,先在1维转角上全局投票滤外点,再求平移并做结式精修。嵌进ORB-SLAM2后,KITTI绝对位姿RMSE从16.40降到9.36。
绝对位姿估计要从2D-3D对应里恢复相机旋转和平移,也就是 PnP。真实匹配里外点很多,RANSAC 在高外点率下既慢又不稳;全局最优搜索又被6自由度空间拖住。IMU 几乎总能给出重力方向,旋转只剩绕重力轴的1个自由度,问题从6自由度降到4自由度。已有重力先验方法往往先搜3自由度平移,外点还没滤干净,3维搜索仍然贵,滤完内点后也缺少专门的4自由度精修。
东南大学、慕尼黑工大和澳门城市大学的这篇把顺序倒过来:先在1维转角上做全局投票,再在滤过的对应上求平移,最后用隐变量结式做非最小二乘精修。
重力在相机系和世界系对齐之后,旋转写成绕重力轴的角 θ,加上一个把两系重力转齐的固定矩阵。一对2D-3D对应可以消掉尺度和平移,得到只含 θ 的三角方程。N个点产生 N(N-1)/2 对,每对解一个 θ,投进 [-π, π] 上360个箱子。票最多的箱子就是 θ,经常投进该箱的对应更像内点。
旋转定了,平移变成线性拟合,RANSAC 在已经瘦过的集合上跑。精修把重力对齐到 z 轴,旋转只剩 cosθ、sinθ 两个变量,对平移闭式消去后变成带 x²+y²=1 约束的二次型。拉格朗日条件化成两条圆锥曲线的交点,用隐变量结式收到一个四次方程,选出目标最小的根。
合成实验里,成功定义为旋转误差小于1°、平移误差小于1.5。N=1000、噪声 σ=2.0 时,外点率从10%加到80%,该方法的旋转和平移成功率都明显高于 Gao/AP3P/EPnP 等 RANSAC 方案,也高于重力4自由度的 BnB。噪声对成功率的伤害比外点率更大;在合理噪声带里,旋转和平移成功率接近100%。重力方向再叠0.5°到5°的偏差,误差会跟着长,大偏差时开始出现明显失败。
真实数据在 TUM RGB-D、ETH3D、RobotCar 上用非相邻帧做 ORB/SIFT 匹配,精度曲线画在图里,正文没有逐数据集的误差表。耗时上,N=1000 时单线程约8.05秒,16线程降到0.91秒;旋转投票是二次复杂度,占掉大部分时间。BnB 在 ETH3D 上平均约30秒,该方法约1.47秒。
嵌进 ORB-SLAM2 替换原 PnP,在 KITTI 上绝对位姿误差 RMSE 从16.40降到9.36,相对位姿误差 RMSE 从1.029降到0.539。重定位时,漂移后的轨迹能重新贴回点云地图。
车上、机器人上本来就有重力。先在1维角上投票,外点在进入平移之前就被削掉一截,这比「先搜平移」更吃得到4自由度的便宜。精修是为这个约束写的,不是套一层通用 PnP。KITTI 上的 RMSE 腰斩,说明它能进现成 SLAM,不只是合成榜。
它不是学习方法,没有训练,外点再高也跑得完。代价是对应数量一大,投票就是 O(N²)。
真实集精度主要靠图,缺少和基线对齐的数值表,复述只能落到 KITTI 和耗时。重力偏了,位姿跟着偏,论文没有把 IMU 噪声建成可部署的补偿。N=1000 单线程仍要数秒,实时要靠多线程或先降采样。投票箱子是1°一格的硬量化,精修能补一点,极端噪声下仍可能投错箱。源码写的是接收后公开。