一种快速稳健的单目相机位姿估计算法制造技术

本发明专利技术涉及一种快速稳健的单目相机位姿估计算法，所述方法包括使用S估计方法迭代计算标志点权值，通过加权正交迭代算法计算世界坐标系相对于相机的旋转矩阵和平移向量得到位姿，如此不断循环往复，直至收敛，得到优化后的位姿参数。本发明专利技术不仅可以自动识别粗差点、提高算法的稳健性，而且能实时、精确地求得旋转矩阵和平移向量，获得准确、可靠的单目相机位姿参数。位姿参数。位姿参数。

【技术实现步骤摘要】
一种快速稳健的单目相机位姿估计算法


[0001]本专利技术涉及位姿估计
，具体说是一种快速稳健的单目相机位姿估计算法。

技术介绍

[0002]在已知单目相机内参数的情况下，根据给定的n个标志点在世界坐标系W中的三维坐标和对应的二维图像像素坐标，求解坐标系W相对于相机坐标系的旋转矩阵R和平移向量t的问题，称为2D
‑
3D位姿估计问题，也称为n点透视(PnP)问题(文献[1])。
[0003]高效n点透视算法(EPnP)是第一个复杂度为O(n)的快速单目位姿估计算法(文献[2])，EPnP方法通过最小化代数误差，可以在线性复杂度的时间内解算旋转和平移参数。直接最小二乘位姿估计算法(DLS)(文献[3])和最优PnP算法(OPnP)(文献[4])是可以快速、精确、能够处理多解情况的算法，这两种算法均采用多项式结式的方法得到了像方残差的解析解，基于多项式结式的方法往往提供了很多可能的位姿估计解，其中的绝大多数可能解都是不符合要求的。统一PnP算法(UPnP)(文献[5])是一种能够同时处理中心投影模型和非中心投影模型的、复杂度为O(n)的几何最优算法。正交迭代算法(OI方法)(文献[6])是一种经典的广泛使用的迭代算法，该方法提出了一种基于物方残差的位姿估计算法，使用奇异值分解方法得到位姿参数，OI算法具有精度高和计算速度快的优点，是一种被广泛的应用的单目位姿估计方法。近年来，人们对OI方法进行了多方面的改进和推广应用。文献[7]将正交迭代算法应用于多目相机的位姿估计中，文献[8]对比了正交迭代算法与其他位姿估计算法的计算效率，文献[9]提出了提高正交迭代算法速度的方法，文献[10]和文献[11]基于加权形式的物方残差模型，分别提出了加权正交迭代算法(W
‑
OI)和基于尺度估计的加权正交迭代算法(S
‑
OI)。稳健PnP位姿估计算法(RPnP)是一种复杂度为O(n)的位姿估计算法(文献[12])，RPnP的基本思路是随机选择n条以标志点为端点的线段，从中选择一条长度最长的线段，以该线段中心为坐标原点，该线段为Z轴建立新的坐标系，利用两点间距离等式平方作为约束条件，通过求解驻点方法得到位姿参数结果，该方法的稳健性来源主要是在线段越长，端点的误差对线段的影响越小。然而当遇到大范围的粗差时，通过挑选最长线段的方法并不能增强算法的稳健性。
[0004]在实际应用过程中，受测量精度限制，世界坐标系W中的三维坐标和对应的二维图像像素坐标等测量值存在测量误差，同时，世界点坐标记录错误、世界点与图像点匹配错误、提取标志点像点坐标时发生较大偏差等情况会不同程度带来测量粗差，导致解算结果不可靠。在测量误差与粗差不可避免的情况下，如何得到实时的、精确的旋转矩阵和平移向量参数，是单目位姿估计算法在应用过程中需解决的重要问题。
[0005]其中，
[0006]文献[1]:Haralick R M,Joo H,Lee C N,et al.Pose estimation from corresponding point data[M]//Freeman H.Machine vision for inspection and measurement.Salt Lake City:Academic Press,1989:1
‑
84.
[0007]文献[2]:Lepetit V,Moreno
‑
Noguer F,Fua P.EPnP:an accurate O(N)solution to the PnP problem[J].International Journal of Computer Vision,2009,81(2):155
‑
166.
[0008]文献[3]:Hesch J A,Roumeliotis S I.A direct least
‑
squares(DLS)method for PnP[C]//2011International Conference on Computer Vision,November 6
‑
13,2011,Barcelona,Spain.New York:IEEE,2011:383
‑
390.
[0009]文献[4]:Zheng Y,Kuang Y,Sugimoto S,et al.Revisiting the PnP Problem:AFast,General and Optimal Solution:IEEE International Conference on Computer Vision,2013[C].
[0010]文献[5]:Kneip L,Li H D,Seo Y.UPnP:an optimal O(N)solution to the absolute Pose problem with universal applicability[M]//Fleet D,Pajdla T,Schiele B,et al.Computer vision
–
ECCV 2014.Lecture notes in computer science.Cham:Springer,2014,8689:127
‑
142.
[0011]文献[6]:Lu C P,Hager G D,Mjolsness E.Fast and globally convergent Pose estimation from video images[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2000,22(6):610
‑
622.
[0012]文献[7]:Xu Y X,Jiang Y L,Chen F.Generalized orthogonal iterative algorithm for Pose estimation of multiple camera systems[J].Acta Optica Sinica,2009,29(1):72
‑
77.
[0013]许允喜,蒋云良,陈方.多摄像机系统位姿估计的广义正交迭代算法[J].光学学报,2009,29(1):72
‑
77.
[0014]文献[8]:Liu J B,Guo P Y,Li X,et al.Evaluation strategy for camera Pose estimation algorithm based on point correspondences[J].Acta Optica Sinica,2016,36(5):0515002.
[0015]刘进博,郭鹏宇,李鑫,等.基于点对应的相机姿态估计算法性能评价[J].光学学报,2016,36(5):051500本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种快速稳健的单目相机位姿估计算法，其特征在于，包括以下步骤：步骤1，将每个标志点的权值设置为记第i个标志点在W坐标系中的坐标为p
i
，在归一化坐标平面中的坐标为v
i
(i＝1,2,
…
,n)，理想情况下相机成像的共线方程为Rp
i
+t＝V
i
(Rp
i
+t)，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)称为投影矩阵，采用迭代方法求解旋转矩阵和平移向量，当第i个标志点的权值固定为ω
i
时，其中ω
i
≥0且使用加权物方残差和函数作为优化目标函数求解位姿参数；步骤2，将旋转矩阵初始化为三阶单位矩阵，通过公式(6)不断正交迭代计算旋转矩阵R
(k)
，直到物方残差小于阙值或正交迭代次数超过设定的最大迭代次数，输出当前的旋转矩阵，通过公式(3)计算对应的平移向量t
(k)
，具体如下，假设第k次迭代时的旋转矩阵为R
(k)
，当R
(k)
固定时，使得加权物方残差和优化目标函数(2)最小的平移向量为第k+1次迭代时的旋转矩阵为其中记其中表示矩阵的张量积，用r
(k)
＝ve(cR
(k)
)表示将旋转矩阵R
(k)
的每一列依次排列形成的一个列向量。通过矩阵G，可将公式(2)重新写成t
(k)
＝Gr
(k)
，同理可将改写成其...

【专利技术属性】
技术研发人员：张雄锋，
申请(专利权)人：中国人民解放军六三六二八部队，
类型：发明
国别省市：