基于点和直线段对应的扩展正交迭代位姿估计算法

正交迭代算法是基于点特征,全局且快速收敛的位姿估计算法,是目前性能最优的实时位姿估计算法之一。对摄像机观测到的直线段提出了其不确定性描述的新方法:目标空间直线段误差,并把该误差融入到正交迭代算法的位姿计算过程中,形成扩展正交迭代算法。该算法可以同时利用点和直线段特征。算法先把点共面性方程表达为与正交迭代算法中点共线性方程一致的数学形式,再根据目标空间点共面性误差定义了目标空间直线段共面性误差。接着把目标空间点共线性误差和直线段共面性误差二者之和作为误差函数。最后推导出使该函数最小化的迭代求解过程。实验结果表明,本文算法是有效的,精确的。与正交迭代算法的计算结果对比,由于可以同时利用直线段特征,扩展正交迭代算法的位姿估计误差更低,抗噪声性能更强,算法更稳定。