文章快速检索     高级检索
  浙江大学学报(工学版)  2018, Vol. 52 Issue (1): 151-159  DOI:10.3785/j.issn.1008-973X.2018.01.020
0

引用本文 [复制中英文]

张振杰, 李建胜, 赵漫丹, 张小东. 基于三视图几何约束的摄像机相对位姿估计[J]. 浙江大学学报(工学版), 2018, 52(1): 151-159.
dx.doi.org/10.3785/j.issn.1008-973X.2018.01.020
[复制中文]
ZHANG Zhen-jie, LI Jian-sheng, ZHAO Man-dan, ZHANG Xiao-dong. Camera pose estimation based on three-view geometry constraint[J]. Journal of Zhejiang University(Engineering Science), 2018, 52(1): 151-159.
dx.doi.org/10.3785/j.issn.1008-973X.2018.01.020
[复制英文]

基金项目

国家自然科学基金资助项目(2015AA7034057A)

作者简介

作者简介:张振杰(1988-), 男, 博士生, 从事视觉导航的研究.
orcid.org/0000-0001-6557-0758.
Email: zzjxiaodao@126.com

通信联系人

李建胜, 男, 教授.
orcid.org/0000-0002-5216-503X.
Email: ljszhx@vip.sina.com

文章历史

收稿日期:2017-09-07
基于三视图几何约束的摄像机相对位姿估计
张振杰, 李建胜, 赵漫丹, 张小东     
信息工程大学 导航与空天目标工程学院, 河南 郑州 450000
摘要: 针对从影像中恢复摄像机位姿的问题,提出基于三视图几何约束的位姿估计算法.利用强制选择机制对影像进行特征点提取,通过光流法和前后向误差实现相邻三帧影像的特征点匹配;推导基于本质矩阵优化分解的位姿估计,利用对极几何约束构建目标函数,通过迭代优化确定旋转矩阵的唯一解,优化了唯一解的确定方法,提高了相对位姿估计效率,得到第1、2摄像机矩阵;基于三视图的几何约束关系,由三焦点张量和匹配特征点建立目标函数,由迭代过程得到第3摄像机相对于第1摄像机的位姿参数.结果表明,提出算法的鲁棒性、精度以及算法效率均优于传统算法,能够快速、准确地估计摄像机相对位姿,可以实现对旋翼无人机的轨迹跟踪.
关键词: 位姿估计    对极几何    三视图几何    目标函数    迭代过程    
Camera pose estimation based on three-view geometry constraint
ZHANG Zhen-jie , LI Jian-sheng , ZHAO Man-dan , ZHANG Xiao-dong     
School of Navigation and Aerospace Engineering, Information Engineering University, Zhengzhou 450000, China
Abstract: A novel pose estimation method based on three-view geometry constraint was proposed in order to solve camera pose estimation from image sequence. Image feature points were detected by using brute force selection mechanism, and feature points were matched based on optical flow algorithm and forward-backward error. Then pose estimation based on improved essential matrix decomposition was deduced. Object function was established according to epipolar geometry constraint, and rotation matrix was iteratively computed. Then a unique solution was obtained. The first and second camera projective matrices were obtained. The object function was built based on trifocal tensor and matching points, and the third camera pose matrix that was relative to the first camera was iteratively computed. Results show that the proposed method improves the robust, position accuracy and efficiency. The method can achieve a precision estimation fast and estimate the trajectory of rotor UAV.
Key words: pose estimation    epipolar geometry    three-view geometry    object function    iterative process    

从两幅视图或多幅视图恢复摄像机相对位姿或摄像机矩阵是计算机视觉研究的经典问题,也是视觉定位[1-2]、自主驾驶[3]和视觉导航[4]等技术的重要研究内容.本文以旋翼无人机的位姿估计为研究对象,基于序列影像对无人机进行定位,辅助无人机实现自主导航.视觉定位问题可以通过视觉里程计[5]和视觉SLAM[6-7]的方法进行解决.在现有的视觉里程计和视觉SLAM方法中,相对位姿的估计是其中的重要步骤和研究重点.研究鲁棒性更好、效率更高的相对位姿估计算法是视觉定位技术的重要内容.

从两视图恢复摄像机相对位姿应用较广泛,研究较多.传统算法通过对两视图的本质矩阵进行SVD奇异值分解,得到摄像机相对位姿[8].该方法依赖于本质矩阵的SVD奇异值分解;需要对场景特征点进行三维重构,利用重构点的约束从4组解中确定唯一解.李国栋等[9]提出不依赖矩阵奇异值分解运算的本质矩阵分解相对位姿和唯一解确定算法.该方法对传统算法进行改进,但需要对4组解确定唯一解.

相较于两视图对极几何约束,三视图几何约束信息更丰富,鲁棒性更好.对于短基线情况,三视图几何约束模型优于对极几何模型.从三视图恢复摄像机相对位姿是研究的重要方向.Hartley等[8, 10]研究从三焦点张量恢复摄像机矩阵的算法.Guerrero等[11]针对纯测角数据,提出基于1-D三焦点张量的机器人定位算法.Lopez-Nicolas等[12]提出基于三焦点张量的机器人视觉控制.Kitt等[13]提出基于三焦点张量求解摄像机运动参数的算法,实现车辆运动参数的估计.Jia等[14]提出基于三焦点张量的移动机器人视觉轨迹跟踪方法.Indelman等[15]提出基于三视图几何的视觉辅助导航方法,结合惯性导航系统,实现了车辆的位置估计.现有的大部分基于三焦点张量的位姿估计算法都需要对表示三视图几何关系的三焦点张量进行求解,再对三焦点张量进行分解得到位姿参数.算法依赖于三焦点张量的计算和分解,算法的稳定性较差,效率较低.

本文在现有算法研究的基础上,提出无需计算三焦点张量的三视图相对位姿估计算法.相比于现有算法,提出的算法充分利用两视图对极几何约束和三视图几何约束,优化了本质矩阵分解相对位姿的方法,避免了三焦点张量的计算,提高了相对位姿估计的鲁棒性和效率.

1 问题描述

传统的相对位姿估计算法大多通过两视图的本质矩阵分解和三视图三焦点张量分解,得到摄像机的相对位姿参数.如图 1所示,mm′和m″为空间点M在摄像机1、2、3位置下的投影点.给定三帧影像对应的3个摄像机矩阵,摄像机1矩阵P =K [I |0], 摄像机2矩阵P′=K [R′|t′]和摄像机3矩阵P″=K [R″|t″].其中,I为3×3的单位阵,K为相机的内参数矩阵,[R′|t′]和[R″|t″]分别为摄像机2和摄像机3相对于摄像机1的位姿参数.

图 1 三视图的几何关系 Fig. 1 Three-view geometry

匹配点集的任意一组匹配点对(m,m′,m″),齐次坐标为m =[x, y, 1]Tm′=[x′, y′, 1]Tm″=[x″, y″, 1]T.对匹配点对坐标进行归一化,即$\mathit{\boldsymbol{\hat m}}$ =K-1m, $\mathit{\boldsymbol{\hat m}}$ ′=K-1m′, $\mathit{\boldsymbol{\hat m}}$ ″=K-1m″.

($\mathit{\boldsymbol{\hat m}}$, $\mathit{\boldsymbol{\hat m}}$′)满足式:$\mathit{\boldsymbol{\hat m}}$TE $\mathit{\boldsymbol{\hat m}}$ =0.其中,E为本质矩阵,由相机的运动参数R′和t′确定,与内参数无关,有E =[t′]×R′.对本质矩阵进行奇异值分解后,可以得到运动参数R′和相差一个常数因子情况下的t′.

($\mathit{\boldsymbol{\hat m}}$, $\mathit{\boldsymbol{\hat m}}$ ′, $\mathit{\boldsymbol{\hat m}}$″)满足式:${\left[{\mathit{\boldsymbol{\hat m'}}} \right]_ \times }\left( {\sum\limits_i {{{\mathit{\boldsymbol{\hat m}}}^i}{\mathit{\boldsymbol{T}}_i}} } \right){\left[{\mathit{\boldsymbol{\hat m''}}} \right]_ \times } = $ =03×3.其中{T1,T2,T3}为三焦点张量,${\mathit{\boldsymbol{\hat m}}}$ = ${\left[{{{\mathit{\boldsymbol{\hat m}}}^1}, {{\mathit{\boldsymbol{\hat m}}}^2}, {{\mathit{\boldsymbol{\hat m}}}^3}} \right]^{\rm{T}}}$.由三幅视图的匹配特征点对可以估计得到三焦点张量{T1,T2,T3}.通过对三焦点张量进行分解,可以得到第2摄像机和第3摄像机相对于第1摄像机的位姿[R′,t′]和[R″,t″].该方法首先需要进行三焦点张量计算,并由三焦点张量计算本质矩阵,再由本质矩阵分解得到相对位姿.该算法复杂且矩阵运算较多,算法效率较低.

2 算法实现

基于三视图几何约束的位姿估计是对三帧影像基于对极几何和三视图几何约束进行处理得到摄像机的相对位姿,然后将摄像机位置参数转换到全局坐标系中,最后得到摄像机的空间运动轨迹.以5帧影像为例,具体实现如图 2所示.

图 2 基于三视图几何约束的位姿估计算法实现流程图 Fig. 2 Flow chart of pose estimation algorithm based on three-view geometry constraint

该算法主要包括3个步骤:利用金字塔光流法和前反向误差,对连续三帧影像进行特征点匹配;然后基于两视图对极几何约束对本质矩阵进行分解,得到摄像机相对位姿,即确定了第1和第2摄像机矩阵;在已知第1和第2摄像机矩阵的基础上,利用三视图几何约束建立目标函数,通过迭代优化求解第3摄像机相对于第1摄像机的相对位姿.

2.1 基于光流和前后向误差的三视图特征点匹配

特征点检测和匹配是制约算法精度和算法效率的主要问题,因此关于特征点检测和匹配算法的研究一直是计算机视觉研究的重点.针对本文研究的问题,邻帧影像的变化较小,对实时性的要求较高,采用特征点强制选择机制、金字塔LK光流法以及前、后向误差实现了三视图特征点的检测和匹配.

1) 特征点强制选择机制[16].

特征点强制选择机制是强制选择影像上的像素点作为影像的特征点,强制选择的特征点可以是影像上的所有像素点,也可以是均匀采样的像素点.强制选择机制避免了复杂的特征点检测过程,大大地提高了算法效率,但该方法的特征点检测精度较低.

2) 基于金字塔LK光流和前后向误差[16]的特征点匹配.

金字塔LK(Lucas-Kanade)光流法可以在较大的空间尺度上对相邻帧影像进行特征点跟踪.通过金字塔LK光流法可以得到三视图的初始匹配特征点集,但光流法受光照、形变等的影响较大,初始匹配结果中的误匹配较多,因此需要进一步处理初始匹配结果,剔除误匹配特征点.

前后向误差的目的是剔除光流跟踪错误的点,提高光流跟踪的鲁棒性,如图 3所示,具体的实现过程如下.

图 3 前后向误差 Fig. 3 Forward-backward error

1) S =(Ii,Ii+1,Ii+2)是三帧图像序列,xi是影像Ii上特征点的位置.

2) 通过光流法对xi进行前向跟踪,得到跟踪结果Tf=[xi, xi+1, xi+2],其中f表示前向跟踪.

3) 对点xi+2通过光流法再进行反向跟踪直到第1帧,从而得到跟踪结果Tb=[${\mathit{\boldsymbol{\hat x}}}$i, ${\mathit{\boldsymbol{\hat x}}}$i+1, ${\mathit{\boldsymbol{\hat x}}}$i+2],${\mathit{\boldsymbol{\hat x}}}$i+2=xi+2.

4) 反向误差定义为前、后向对应点跟踪结果的距离,Bi=d(xi, ${\mathit{\boldsymbol{\hat x}}}$i),单位为像素.采用回溯点与原点之间的欧式距离作为反向误差,d(xi, ${\mathit{\boldsymbol{\hat x}}}$i)=‖xi-${\mathit{\boldsymbol{\hat x}}}$ i‖.

5) 选择合适的阈值dt,对跟踪错误的点进行剔除.阈值dt的选择影响了特征点匹配的个数和准确性.阈值较小,特征点匹配数量较少但准确性高;阈值较大,特征点匹配数量较多但准确性低.本文选择阈值dt=1像素.

2.2 基于对极几何的两视图相对位姿估计

以旋翼无人机为研究对象,利用相对姿态变换较小的特点,提出优化本质矩阵分解的相对位姿估计算法,大大提高了位姿解算的效率.

该算法主要包括3个步骤:由本质矩阵性质确定平移向量的解;建立目标函数利用迭代算法估计旋转矩阵;推导了唯一解确定的判断条件,优化了唯一解确定方法.

1) 相对位姿估计.

由匹配特征点对可以估计得到基本矩阵F,进而可以得到本质矩阵E.根据文献[9]的方法,可以确定平移向量的2个解t =[tx, ty, tz]Tt =-[tx, ty, tz]T.确定平移向量的估计值后,将旋转矩阵R作为未知解进行解算.

本质矩阵的表达式E =[t]×R,矩阵E和矩阵[t]×R在相差一个尺度因子下相等.平移向量t满足‖t2=1,R为单位正交矩阵,因此矩阵[t]×R的2范数为‖[t]×R2=2.对本质矩阵E进行缩放变化,使E满足‖E2=2.此时,矩阵E和矩阵[t]×R的各个元素在相差一个正、负号的情况下对应相等.正、负值可以在算法的迭代过程中进行确定.矩阵E和矩阵[t]×R满足式E -[t]×R =03×3.R的求解是目标函数(1)寻优的过程.

$ \mathit{\boldsymbol{R}} = \arg \mathop {\min }\limits_\mathit{\boldsymbol{R}} {\left\| {\mathit{\boldsymbol{E}} - {{\left[ \mathit{\boldsymbol{t}} \right]}_ \times }\mathit{\boldsymbol{R}}} \right\|^2}. $ (1)

R使用李群方法表示R (ξ)[5]

$ \mathit{\boldsymbol{R}}\left( \mathit{\boldsymbol{\xi }} \right) = \exp \left( {\sum\limits_{j = 1}^3 {{\xi _j}{\mathit{\boldsymbol{G}}_j}} } \right). $ (2)

式中:ξ =[ξ1, ξ2, ξ3]T为运动增量,对应于摄像机的帧间角速度.基矩阵Gj的形式如下:

$ \begin{array}{l} {\mathit{\boldsymbol{G}}_1} = \left[ {\begin{array}{*{20}{c}} 0&0&0\\ 0&0&{ - 1}\\ 0&1&0 \end{array}} \right],{\mathit{\boldsymbol{G}}_2} = \left[ {\begin{array}{*{20}{c}} 0&0&1\\ 0&0&0\\ { - 1}&0&0 \end{array}} \right],\\ {\mathit{\boldsymbol{G}}_3} = \left[ {\begin{array}{*{20}{c}} 0&{ - 1}&0\\ 1&0&0\\ 0&0&0 \end{array}} \right]. \end{array} $ (3)

d (ξ)=E -[t]×R (ξ).对d (ξ)进行线性化处理,可得

$ \mathit{\boldsymbol{d}}\left( \mathit{\boldsymbol{\xi }} \right) \approx \mathit{\boldsymbol{d}}\left( 0 \right) + \nabla \mathit{\boldsymbol{d}} \cdot \mathit{\boldsymbol{\xi }}. $ (4)

令雅可比矩阵J =▽d.

$ \mathit{\boldsymbol{J}} = \frac{{\partial \mathit{\boldsymbol{d}}\left( \mathit{\boldsymbol{\xi }} \right)}}{\mathit{\boldsymbol{\xi }}} = {\left[ \mathit{\boldsymbol{t}} \right]_ \times }\left[ {{\mathit{\boldsymbol{G}}_1},{\mathit{\boldsymbol{G}}_2},{\mathit{\boldsymbol{G}}_3}} \right], $ (5)

Q1=[t]×G1Q2=[t]×G2Q3=[t]×G3,则式(5)可以进一步改写为

$ {\mathit{\boldsymbol{J}}_{9 \times 3}} = \left[ {\begin{array}{*{20}{c}} {{{\left( {{\mathit{\boldsymbol{Q}}_1}} \right)}_1}}&{{{\left( {{\mathit{\boldsymbol{Q}}_2}} \right)}_1}}&{{{\left( {{\mathit{\boldsymbol{Q}}_3}} \right)}_1}}\\ {{{\left( {{\mathit{\boldsymbol{Q}}_1}} \right)}_2}}&{{{\left( {{\mathit{\boldsymbol{Q}}_2}} \right)}_2}}&{{{\left( {{\mathit{\boldsymbol{Q}}_3}} \right)}_2}}\\ {{{\left( {{\mathit{\boldsymbol{Q}}_1}} \right)}_3}}&{{{\left( {{\mathit{\boldsymbol{Q}}_2}} \right)}_3}}&{{{\left( {{\mathit{\boldsymbol{Q}}_3}} \right)}_3}} \end{array}} \right]. $ (6)

式中:(Qi)j为矩阵Qi(i=1, 2, 3)的第j列,j=1, 2, 3, (Qi)j为3×1的向量.

式(1)可以转化为最小二乘问题进行求解:

$ {\mathit{\boldsymbol{J}}_{9 \times 3}}^{\rm{T}}{\mathit{\boldsymbol{J}}_{9 \times 3}}\mathit{\boldsymbol{\xi }} = - {\mathit{\boldsymbol{J}}_{9 \times 3}}^{\rm{T}}{\mathit{\boldsymbol{J}}_{9 \times 1}}\left( 0 \right). $ (7)

式中:d9×1(0)为d (0)的向量化.

运动增量ξ可以由下式得到:

$ \mathit{\boldsymbol{\hat \xi }} = - {\left( {{\mathit{\boldsymbol{J}}_{9 \times 3}}^{\rm{T}}{\mathit{\boldsymbol{J}}_{9 \times 3}}} \right)^{ - 1}}{\mathit{\boldsymbol{J}}_{9 \times 3}}^{\rm{T}}{\mathit{\boldsymbol{J}}_{9 \times 1}}. $ (8)

进而可以得到R的估计值.通过迭代计算可以得到R的最优解.

2) 确定唯一解.

现有方法分解出来的解均为4组解,需要对4组解进行判断,进而确定唯一解.传统方法是对特征点进行三维重建,通过三维点深度信息来确定.李国栋等[9]对传统方法进行改进,直接求解三维点的深度信息,避免了进行三维重建.

利用相对位姿估计方法计算得到的旋转矩阵R是唯一确定的,因此只有2组解[R,t]和[R, -t],只需对2组解进行判断.通过空间直线交点计算,给出一种快速判断方法.

为了方便计算,将第2摄像机O′坐标系作为世界坐标系.如图 1所示,光心O和投影点m的投影线$\overline {Om} $,方向向量为N =[n1, n2, n3]T,光心O′和投影点m′的投影线$\overline {O'm'} $,方向向量为N ′=[n1′, n2′, n3′]T,它们为同名光线.mm′在第2摄像机下的坐标分别为mc=R [x, y, f]T+tmc′=[x′, y′, f]T.因此,N ′、N可以表示为:N ′=mc′,N =R [x, y, f]T.空间点M [X, Y, Z]T是2条空间直线$\overline {Om} $$\overline {O'm'} $的交点.空间直线$\overline {Om} $$\overline {O'm'} $的直线方程如下:

$ \left. \begin{array}{l} \frac{{X - {t_x}}}{{{n_1}}} = \frac{{Y - {t_y}}}{{{n_2}}} = \frac{{Z - {t_z}}}{{{n_3}}},\\ \frac{X}{{{{n'}_1}}} = \frac{Y}{{{{n'}_2}}} = \frac{Z}{{{{n'}_3}}} = \lambda . \end{array} \right\} $ (9)

位姿唯一解是通过找到使重构点同时在两个摄像机的前面进行确定的,本文的唯一解是满足λn3′>0和λn3′>tz的解.

λ可以由公式λ=(n2tx-n1ty)/(n1n2-n1n2′)解算得到.

2.3 基于三视图几何约束的相对位姿迭代估计

基于三视图几何约束的相对位姿迭代估计是在已知第1和第2摄像机矩阵的情况下,利用三视图几何约束,即三焦点张量,建立目标函数,通过寻找目标函数的极值估计得到第3摄像机相对于第1摄像机的位姿.

在确定前两帧影像的相对位姿后,即已知第1和第2摄像机矩阵P =K [I |0]和P′=K [R′|t′],由已确定的前两个摄像机矩阵以及三视图的几何约束,可以解算得到第3摄像机矩阵P″=K [R″|t″].三视图的匹配特征点满足下式:

$ {\left[ {\mathit{\boldsymbol{\hat m'}}} \right]_ \times }\left( {\sum\limits_i {{{\mathit{\boldsymbol{\hat m}}}^i}{\mathit{\boldsymbol{T}}_i}} } \right){\left[ {\mathit{\boldsymbol{\hat m''}}} \right]_ \times } = {{\bf{0}}_{3 \times 3}}. $ (10)

式中:${\mathit{\boldsymbol{\hat m}}}$${\mathit{\boldsymbol{\hat m}}}$ ′和${\mathit{\boldsymbol{\hat m}}}$ ″分别为三视图上的归一化特征点,${\mathit{\boldsymbol{\hat m}}}$ =[${\mathit{\boldsymbol{\hat m}}}$1, ${\mathit{\boldsymbol{\hat m}}}$2, ${\mathit{\boldsymbol{\hat m}}}$3]T,[T1,T2,T3]为三焦点张量.对式(10)进一步整理,可得

$ {\left[ {\mathit{\boldsymbol{\hat m'}}} \right]_ \times }\mathit{\boldsymbol{R'\hat m}}{{\mathit{\boldsymbol{t''}}}^{\rm{T}}}{\left[ {\mathit{\boldsymbol{\hat m''}}} \right]_ \times } - {\left[ {\mathit{\boldsymbol{\hat m'}}} \right]_ \times }\mathit{\boldsymbol{t'}}{{\mathit{\boldsymbol{\hat m}}}^{\rm{T}}}{{\mathit{\boldsymbol{R''}}}^{\rm{T}}}{\left[ {\mathit{\boldsymbol{\hat m''}}} \right]_ \times } = {{\bf{0}}_{3 \times 3}}. $ (11)

相对位姿R″和t″的求解是寻找满足下式的过程:

$ \begin{array}{l} \left( {\mathit{\boldsymbol{R''}},\mathit{\boldsymbol{t''}}} \right) = \\ \;\;\;\;\;\;\arg \mathop {\min }\limits_{\mathit{\boldsymbol{R''}},\mathit{\boldsymbol{t''}}} \frac{1}{2}\sum\limits_{i = 1}^n {\left\| {{{\left[ {{{\mathit{\boldsymbol{\hat m'}}}_i}} \right]}_ \times }\mathit{\boldsymbol{R'}}{{\mathit{\boldsymbol{\hat m}}}_i}{{\mathit{\boldsymbol{t''}}}^{\rm{T}}}{{\left[ {{{\mathit{\boldsymbol{\hat m''}}}_i}} \right]}_ \times } - } \right.} \\ \;\;\;\;\;\;{\left. {{{\left[ {{{\mathit{\boldsymbol{\hat m'}}}_i}} \right]}_ \times }\mathit{\boldsymbol{t'\hat m}}_i^{\rm{T}}{{\mathit{\boldsymbol{R''}}}^{\rm{T}}}{{\left[ {{{\mathit{\boldsymbol{\hat m''}}}_i}} \right]}_ \times }} \right\|^2}. \end{array} $ (12)

R″使用李群方法可以表示为

$ \mathit{\boldsymbol{R''}}\left( \mathit{\boldsymbol{\xi }} \right) = \exp \left( {\sum\limits_{j = 1}^3 {{\xi _j}{\mathit{\boldsymbol{G}}_j}} } \right). $ (13)

$ \begin{array}{l} {\mathit{\boldsymbol{d}}_i}\left( {\mathit{\boldsymbol{\xi }},\mathit{\boldsymbol{t''}}} \right) = {\left[ {{{\mathit{\boldsymbol{\hat m'}}}_i}} \right]_ \times }\mathit{\boldsymbol{R'}}{{\mathit{\boldsymbol{\hat m}}}_i}{{\mathit{\boldsymbol{t''}}}^{\rm{T}}}{\left[ {{{\mathit{\boldsymbol{\hat m''}}}_i}} \right]_ \times } - {\left[ {{{\mathit{\boldsymbol{\hat m'}}}_i}} \right]_ \times }\mathit{\boldsymbol{t'\hat m}}_i^{\rm{T}} \times \\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{{\mathit{\boldsymbol{R''}}}^{\rm{T}}}\left( \mathit{\boldsymbol{\xi }} \right){\left[ {{{\mathit{\boldsymbol{\hat m''}}}_i}} \right]_ \times }, \end{array} $

其中di(ξ,t″)是3×3的矩阵,将di(ξ,t″)改写为

$ \begin{array}{l} {d^{jk}}\left( {\mathit{\boldsymbol{\xi }},\mathit{\boldsymbol{t''}}} \right) = {\left[ {{{\mathit{\boldsymbol{\hat m'}}}_i}} \right]_ \times }^j\mathit{\boldsymbol{R'\hat m}}{{\mathit{\boldsymbol{t''}}}^{\rm{T}}}{\left[ {{{\mathit{\boldsymbol{\hat m''}}}_i}} \right]_{ \times k}} - \\ \;\;\;\;{\left[ {\mathit{\boldsymbol{\hat m'}}} \right]_ \times }^j\mathit{\boldsymbol{t'}}{{\mathit{\boldsymbol{\hat m}}}^{\rm{T}}}{{\mathit{\boldsymbol{R''}}}^{\rm{T}}}\left( \mathit{\boldsymbol{\xi }} \right){\left[ {\mathit{\boldsymbol{\hat m''}}} \right]_{ \times k}}. \end{array} $ (14)

式中:j表示矩阵[${\mathit{\boldsymbol{\hat m}}}$ ′]×的行,j=1, 2, 3;k表示矩阵[${\mathit{\boldsymbol{\hat m}}}$ ″]×的列,k=1, 2, 3;djk表示矩阵的第jk列.

djk(ξ,t″)进行线性化处理,可得

$ {d^{jk}}\left( {\mathit{\boldsymbol{\xi }},\mathit{\boldsymbol{t''}}} \right) = {d^{jk}}\left( {0,0} \right) + \frac{{\partial {d^{jk}}\left( {\mathit{\boldsymbol{\xi }},\mathit{\boldsymbol{t''}}} \right)}}{{\partial \mathit{\boldsymbol{t''}}}}{\rm{d}}\mathit{\boldsymbol{t'' + }}\frac{{\partial {d^{jk}}\left( {\mathit{\boldsymbol{\xi }},\mathit{\boldsymbol{t''}}} \right)}}{{\partial \mathit{\boldsymbol{\xi }}}}\mathit{\boldsymbol{\xi }}. $ (15)

式中:

$ \frac{{\partial {d^{jk}}\left( {\mathit{\boldsymbol{\xi }},\mathit{\boldsymbol{t''}}} \right)}}{{\partial \mathit{\boldsymbol{t''}}}} = {\left[ {\mathit{\boldsymbol{\hat m'}}} \right]_ \times }^j\mathit{\boldsymbol{R'\hat m}}{\left[ {\mathit{\boldsymbol{\hat m''}}} \right]_{ \times k}}, $
$ \frac{{\partial {d^{jk}}\left( {\mathit{\boldsymbol{\xi }},\mathit{\boldsymbol{t''}}} \right)}}{{\partial \mathit{\boldsymbol{\xi }}}} = {\left[ {\mathit{\boldsymbol{\hat m'}}} \right]_ \times }^j\mathit{\boldsymbol{t'}}{{\mathit{\boldsymbol{\hat m}}}^{\rm{T}}}\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{G}}_1}}&{{\mathit{\boldsymbol{G}}_2}}&{{\mathit{\boldsymbol{G}}_3}} \end{array}} \right]{\left[ {\mathit{\boldsymbol{\hat m''}}} \right]_{ \times k}}. $

令雅可比矩阵为

$ {\mathit{\boldsymbol{J}}^{jk}} = \left[ {\begin{array}{*{20}{c}} {\frac{{\partial {d^{jk}}\left( {\mathit{\boldsymbol{\xi }},\mathit{\boldsymbol{t''}}} \right)}}{{\partial \mathit{\boldsymbol{\xi }}}}}&{\frac{{\partial {d^{jk}}\left( {\mathit{\boldsymbol{\xi }},\mathit{\boldsymbol{t''}}} \right)}}{{\partial \mathit{\boldsymbol{t''}}}}} \end{array}} \right]. $

Jjk可以构成雅可比矩阵J,可以将式(12)的优化问题转化为最小二乘问题进行求解:

$ {\mathit{\boldsymbol{J}}^{\rm{T}}}\mathit{\boldsymbol{J}}\left[ \begin{array}{l} \mathit{\boldsymbol{\xi }}\\ {\rm{d}}\mathit{\boldsymbol{t''}} \end{array} \right] = - {\mathit{\boldsymbol{J}}^{\rm{T}}}\mathit{\boldsymbol{d}}\left( {0,0} \right). $ (16)

式中:d =[d1, d2, …, dn]T.式(16)的最小二乘解为

$ \left[ \begin{array}{l} {\mathit{\boldsymbol{\hat \xi }}}\\ {\rm{d}}\mathit{\boldsymbol{t''}} \end{array} \right] = - {\left( {{\mathit{\boldsymbol{J}}^{\rm{T}}}\mathit{\boldsymbol{J}}} \right)^{ - 1}}{\mathit{\boldsymbol{J}}^{\rm{T}}}\mathit{\boldsymbol{d}}, $

可以得到R的估计值和t″的估计值.通过迭代计算可以得到Rt″的最优解.

2.4 算法效率分析

该算法的主要过程包括特征提取与匹配、两视图相对位姿估计和三视图相对位姿估计.算法效率的分析可以分为以下3部分进行讨论.

1) 特征点提取与匹配.

特征点提取与匹配是限制整个算法实时性的重要技术.本文方法是基于光流和前、后向误差的三视图特征点匹配.该方法使用特征点强制选择机制,虽然损失了特征点检测精度,但大大降低了算法的耗时,为整个算法的实时性提供了保证.通过利用前、后向误差进行匹配点提纯,避免使用效率低下的RANSAC算法,是算法效率得到提高的重要保证.

2) 两视图相对位姿估计.

两视图相对位姿估计主要可以分为以下4步:本质矩阵求解、平移向量估计、旋转矩阵迭代优化估计和唯一解确定.本质矩阵的解算与匹配点的数目有关,算法复杂度为O(n);平移向量估计由本质矩阵推导等式得到,复杂度为O(1);迭代计算过程与匹配点数无关,复杂度为O(1);迭代次数较少,一般不超过3次;唯一解的确定需对n个点进行判断,因此复杂度为O(n).与传统算法[8]和文献[9]的算法相比,本文算法的矩阵运算量较少,只需对两组解进行唯一解判断,因此算法效率得到有效提高.

3) 三视图相对位姿估计.

两视图相对位姿估计主要是迭代优化过程,计算与匹配点的数目有关,因此每次迭代的复杂度为O(n),但收敛速度较快,一般在3次以内.

3 实验与分析 3.1 数学仿真实验

通过数学仿真实验对该算法的有效性和精度进行验证.仿真参数设置如下:摄像机的焦距f=800,主点坐标u0=v0=0;在区域[-10, 10]×[-10, 10]×[5, 10]中,随机生成n个空间三维点;第1摄像机矩阵P0=K [I |0],第2和第3摄像机旋转角(φ, ω, κ)在[-5°, 5°]随机取值,平移向量t =[tx, ty, tz]T分别在[10, 20]随机取值.对像点坐标添加均值为0、噪声方差为σ的高斯噪声.每次实验进行1 000次,取平均值作为实验的结果.

在算法性能上,比较了以下3种算法.1) SVD,位姿参数由传统的本质矩阵SVD分解得到;2) Trifocal,由三焦点张量恢复摄像机相对位姿;3)本文提出的基于三视图几何的位姿估计算法.根据所述3种不同算法求得的第3摄像机相对于第1摄像机的姿态和位置结果与真实值的误差分别表示为

$ \left. \begin{array}{l} {e_{\rm{a}}} = \left\| {\left[ {\hat \varphi ,\hat \omega ,\hat \kappa } \right] - \left[ {\varphi ,\omega ,\kappa } \right]} \right\|,\\ {e_{\rm{t}}} = \left\| {{{\mathit{\boldsymbol{\hat R''}}}^{\rm{T}}}\mathit{\boldsymbol{\hat t''}} - {{\mathit{\boldsymbol{R''}}}^{\rm{T}}}\mathit{\boldsymbol{t''}}} \right\|/\left\| {{{\mathit{\boldsymbol{R''}}}^{\rm{T}}}\mathit{\boldsymbol{t''}}} \right\|. \end{array} \right\} $ (17)

式中:ea为角度误差,et为相对位置误差,R″为第3摄像机相对于第1摄像机的旋转矩阵,t″为第3摄像机相对于第1摄像机的平移向量,${\mathit{\boldsymbol{\hat R''}}}$${\mathit{\boldsymbol{\hat t''}}}$为算法估计值.

图 4所示为在仿真特征点像点坐标上添加高斯噪声方差σσ为[0.0, 5.0],变化步长为0.5,特征点数为100的摄像机位姿估计误差统计结果.图中,Nl为噪声水平.如图 5所示为特征点数n为[50, 500],变化步长为50,高斯噪声方差σ=2.0的摄像机位姿估计误差统计结果.

图 4 特征点噪声的实验结果 Fig. 4 Relationship between pose error and noise level

图 4的实验结果可以看出,在不同的噪声水平下,提出的算法的位姿估计误差是最小的,姿态误差在0.8°以内,定位误差小于1.5%.当噪声水平大于2时,提出算法的精度优于传统算法.

图 5的实验结果可以看出,随着特征点数n的增加,算法的位姿估计误差减小,提出的算法的位姿估计误差是最小的.

图 5 特征点数的实验结果 Fig. 5 Relationship between pose error and number of feature points

图 6所示为不同算法的运行时间tcn之间的关系.从实验结果可以看出,提出的算法相比于SVD算法和三焦点张量分解算法,效率得到了明显提升.综合上述实验结果,可以得到以下结论:提出的算法位姿估计精度和鲁棒性略高于传统算法;在算法效率上,提出的算法显著优于传统算法,能够在精度和效率上实现较好的平衡.

图 6 SVD、Trifocal和本文方法的算法耗时 Fig. 6 Computation time of SVD, Trifocal and proposed method
3.2 真实图像实验

算法的最终目的是利用影像数据解算旋翼无人机的定位结果,进而确定无人机的飞行轨迹,为无人机自主导航提供参数信息.为了验证提出的算法是否能够为无人机提供定位参数,对一组29帧无人机影像数据进行处理,并将不同算法处理的结果与无人机的GPS数据进行比较.

实验所用的无人机为图 7所示的旋翼无人机.无人机搭载的相机为Panasonic DMC-GH3,图像大小为4 608×2 592像素,摄像机焦距f=25 mm,主点坐标xp=-0.12 mm, yp=0.16 mm.

图 7 旋翼无人机 Fig. 7 Rotor UAV

在算法性能上,比较了以下4种算法.1) SURF+ SVD,特征匹配采用精度较高的SURF算子[18],并用RANSAC算法剔除误匹配,位姿估计算法为本质矩阵SVD分解. 2) OF+SVD,特征匹配采用本文的光流跟踪(optical flow,OF)和前后向误差算法,位姿估计算法为本质矩阵SVD分解. 3) Trifocal,特征匹配采用本文的光流跟踪和前后向误差算法,位姿估计由三焦点张量分解得到. 4)提出的基于三视图几何约束的位姿估计算法.

图 8所示为利用不同算法处理29帧影像估计得到的无人机运动轨迹与由GPS数据得到的无人机轨迹的对比图.在实验中,视觉定位的尺度信息由GPS数据确定.需要对GPS数据和视觉定位初始坐标系进行对齐,对齐是通过计算GPS坐标系和视觉坐标系的刚体变化来确定的.具体方法是选取4个以上的GPS数据和对应的视觉定位结果,利用Horn绝对定位方法解算得到2个坐标系间的转换关系:

$ \mathit{\boldsymbol{R}} = \left[ {\begin{array}{*{20}{c}} {0.9997}&{0.0241}&{0.0017}\\ { - 0.0240}&{0.9983}&{ - 0.0536}\\ { - 0.0030}&{0.0536}&{0.9986} \end{array}} \right]\mathit{\boldsymbol{T}} = \left[ {\begin{array}{*{20}{c}} { - 37.66}\\ {299.14}\\ { - 58.41} \end{array}} \right]. $ (18)
图 8 无人机飞行轨迹 Fig. 8 UAV trajectory

图 8可以看出,利用SURF+SVD算法和提出的算法计算得到的结果与GPS数据最吻合,OF+SVD算法基本失效.如图 9所示为每一帧对应的各个算法的无人机定位结果与GPS数据的相对误差.图中,Nf为当前帧.如图 10所示为2种结果较好的算法的定位误差比较,即提出的算法和SURF+SVD算法.从图 910可以看出,提出算法的稳健性和鲁棒性更好,定位误差小于20%.

图 9 SURF+SVD、OF+SVD、Trifocal和提出算法的定位误差 Fig. 9 Position error of SURF+SVD, OF+SVD, Trifocal and proposed method
图 10 SURF+SVD和提出算法的定位误差 Fig. 10 Position error of SURF+SVD and proposed method

为了保证算法的实时性,对图像进行降分辨率处理,处理后的图像大小为1 152×648像素.对29帧影像进行处理,特征点检测匹配算法和位姿估计算法分别使用C++和Matlab编写.对特征点检测匹配算法和位姿估计算法的耗时分别进行统计.如表 1所示为利用上述4种不同算法对29帧序列影像处理后的特征点检测匹配和位姿估计分别所消耗的时间、算法总耗时以及平均单帧耗时的结果对比.如图 11所示为利用2种算法匹配的特征点数和算法时间的分布图.可以看出,SURF算子检测的特征点数较少,小于500;OF算法匹配的点数较多,大多大于500.在算法时间上,OF算法耗时远远小于SURF算子,时间为300~600 ms.

表 1 不同算法的耗时比较 Table 1 Comparison of computation time among different methods
图 11 匹配算法的效率比较 Fig. 11 Computation time of two matching algorithms

表 1中,td为特征点检测匹配时间,tp为位姿估计时间,ta为算法耗时,ts为平均单帧耗时.综合图 11表 1可以看出,提出的算法位姿估计耗时略高于SVD算法,原因是提出的算法检测的特征点数远远多于SURF算子;在特征点检测匹配过程中,提出的算法耗时大大低于SURF算子.从整体来看,提出的算法效率最高,平均处理1帧影像需要0.576 s.

综合实验结果,可以得到以下结论.提出的算法的特征点检测匹配结果较差,不能用于传统SVD算法和三焦点张量分解算法;利用提出的位姿估计算法能够处理较差的特征点检测匹配结果,鲁棒性好于其他算法,定位精度接近SURF+SVD算法;视觉定位作为增量定位,定位误差将随着时间累积,由于实验处理的帧数较少,且基于三视图的位姿估计精度较高,因此累积误差较小;提出的算法在保证定位精度的同时,算法效率得到大大提高,能够实现精度和效率的均衡性.

4 结语

本文以旋翼无人机为研究对象,针对从影像恢复摄像机的相对位姿的问题,提出基于三视图几何约束的摄像机相对位姿估计算法.提出的算法以两视图对极几何约束和三视图几何约束为基础.首先,由强制选择机制、金字塔光流法和前后向误差实现了三视图特征点检测匹配,利用该方法大大提高了算法效率,是实现算法实时性的主要步骤;然后,推导了本质矩阵分解位姿的新方法;最后,利用三视图几何约束,提出迭代估计摄像机位姿的方法.通过仿真实验验证了提出算法的有效性和正确性.相比于本质矩阵SVD算法和三焦点张量分解算法,提出的算法在点数较多的情况下,定位精度和鲁棒性均是最好的,且算法效率得到显著提高.通过无人机影像数据实验验证提出的特征点检测和匹配算法的高效率,提出算法的稳定性更好,实现了效率和精度的均衡性.与GPS数据相比,定位精度小于20%,处理一帧影像平均耗时为0.576 s.

参考文献
[1]
陈明芽, 项志宇, 刘济林. 单目视觉自然路标辅助的移动机器人定位方法[J]. 浙江大学学报:工学版, 2014, 48(2): 285-291.
CHEN Ming-ya, XIANG Zhi-yu, LIU Ji-lin. Assistance localization method for mobile robot based on monocular natural visual landmarks[J]. Journal of Zhejiang University:Engineering Science, 2014, 48(2): 285-291.
[2]
AFIA A B, DEAMBROGIO L, SALOS D, et al. Review and classification of vision-based localization techniques in unknown environments[J]. IET Radar, Sonar and Navigation, 2014, 8(9): 1059-1072. DOI:10.1049/iet-rsn.2013.0389
[3]
HEE L G, FAUNDORFER F, POLLEFEYS M. Motion estimation for self-driving cars with a generalized camera[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. Portland:IEEE, 2013:2746-2753. http://dl.acm.org/citation.cfm?id=2516053
[4]
BLOSCH M, WEISS S, SCARAMUZZA D, et al. Vision based MAV navigation in unknown and unstructured environments[C]//Robotics and automation (ICRA). Anchorage:IEEE, 2010:21-28. http://ieeexplore.ieee.org/xpls/icp.jsp?arnumber=5509920
[5]
CHRISTAN F, MATIA P, DAVIDE S. SVO:fast semi-direct monocular visual odometry[C]//IEEE International Conference on Robotics and Automation (ICRA). Hong Kong:IEEE, 2014:15-22. http://ieeexplore.ieee.org/xpls/icp.jsp?arnumber=6906584
[6]
ENGEL J, SCHOPS T, CREMERS D. LSD-SLAM:large-scale direct monocular SLAM[C]//European Conference on Computer Vision. Zurich:Springer, 2014:834-849. http://link.springer.com/10.1007/978-3-319-10605-2_54
[7]
MUR-ARTAL R, MONTIEL J M M, TARDOS J D. Orb-slam:a versatile and accurate monocular slam system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163. DOI:10.1109/TRO.2015.2463671
[8]
HARTLEY R I, ZISSERMAN A. Multiple view geometry in computer vision[M]. UK: Cambridge University Press, 2004.
[9]
李国栋, 田国会, 王洪君, 等. 弱标定立体图像对的欧式极线校正框架[J]. 光学精密工程, 2014, 22(7): 1955-1961.
LI Guo-dong, TIAN Guo-hui, WANG Hong-jun, et al. Euclidean epipolar rectification frame of weakly calibrated stereo pairs[J]. Optics and Precision Engineering, 2014, 22(7): 1955-1961.
[10]
黄以君, 刘伟军, 赵吉宾. 一种基于三焦点张量的度量重建方法[J]. 仪器仪表学报, 2009, 30(6): 1307-1312.
HUANG Yi-jun, LIU Wei-jun, ZHAO Ji-bin. Approach to metric reconstruction based on trifocal tensor[J]. Chinese Journal of Scientific Instrument, 2009, 30(6): 1307-1312.
[11]
GUERREIO J J, MUROLLO A C, SAGUES C. Localization and matching using the planar trifocal tensor with bearing-only data[J]. IEEE Transactions on Robotics, 2008, 24(2): 494-501. DOI:10.1109/TRO.2008.918043
[12]
LOPEZ-NICOLAS G, GUERRERO J J, SAGUES C. Visual control through the trifocal tensor for nonholonomic robots[J]. Robotics and Autonomous Systems, 2010, 58(2): 216-226. DOI:10.1016/j.robot.2009.09.005
[13]
KITT B, GEIGER A, LATEGAHN H. Visual odometry based on stereo image sequences with RANSAC-based outlier rejection scheme[C]//Intelligent Vehicles Symposium. La Jolla:IEEE, 2010:486-492. http://ieeexplore.ieee.org/xpls/icp.jsp?arnumber=5548123
[14]
JIA B, CHEN J, ZHANG K. Adaptive visual trajectory tracking of nonholonomic mobile robots based on trifocal tensor[C]//2015 IEEE/RSJ International Conference on IEEE Intelligent Robots and Systems (IROS). Hamburg:IEEE, 2015:3695-3700. http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=7353894
[15]
INDELMAN V, GURFIL P, RIBLIN E, et al. Real-time vision-aided localization and navigation based on three-view geometry[J]. IEEE Transactions on Aerospace and Electronic Systems, 2012, 48(3): 2239-2259. DOI:10.1109/TAES.2012.6237590
[16]
KALAL Z, MIKOLAJCZYK K, MATAS J. Forward-backward error:automatic detection of tracking failures[C]//201020th International Conference on Pattern Recognition (ICPR). Istanbul:IEEE, 2010:2756-2759. http://dl.acm.org/citation.cfm?id=1905750
[17]
张跃强, 苏昂, 刘海波, 等. 基于多直线对应和加权最小二乘的位姿估计[J]. 光学精密工程, 2015, 23(6): 1722-1731.
ZHANG Yue-qiang, SU Ang, LIU Hai-bo, et al. Pose estimation based on multiple line hypothesis and iteratively reweighted least squares[J]. Optics and Precision Engineering, 2015, 23(6): 1722-1731.
[18]
BAY H, TUYTELAARS T, VAN G L. Surf:speeded up robust features[C]//European Conference on Computer Vision. Graz:Springer, 2006:404-417.