一种相机位姿鲁棒估计方法

杨阿华, 李学军, 刘 涛, 刘春晓

(1.装备学院研究生管理大队,北京101416; 2.装备学院信息装备系,北京101416; 3.重庆北碚区军事代表室,重庆400700)

一种相机位姿鲁棒估计方法

杨阿华1, 李学军2, 刘 涛2, 刘春晓3

(1.装备学院研究生管理大队,北京101416; 2.装备学院信息装备系,北京101416; 3.重庆北碚区军事代表室,重庆400700)

针对位姿估计算法对鲁棒性和精度要求高的特点,提出了一种鲁棒的高精度位姿估计方法：该方法以3个样本点对为基础,首先假设各空间点到投影中心的距离近似相等,然后根据像点与空间点间的几何约束关系建立方程组并推导迭代公式,进而迭代求解各空间点到投影中心的距离;以此为基础,提出了一种最小二乘绝对定向方法来获取相机的初始位姿。最后根据空间点与像点之间的共线约束进行迭代优化,得到最终的相机位姿。测试表明,该方法在满足假设的条件下能很好地收敛,并达到较高的解算精度。

位姿估计;样本点对;绝对定向;共线约束

位姿估计是摄影测量学、计算机图形学、机器人导航、无人机空中自主加油以及空间交会对接等领域中的一个核心问题。位姿估计是指求解2个三维坐标系之间的相对位置与姿态,包括3个沿坐标轴方向的平移量和3个绕坐标轴的旋转角。基于特征点的单目视觉位姿估计方法是利用单个相机对目标上3个以上的特征点成像,然后利用特征点成像坐标和特征点间的几何位置关系及相机参数解算相机与目标之间的相对位姿。由于其结构简单和易于实现,该方法成为视觉测量领域的研究热点之一,并广泛应用于三维测量[1-2]和三维拼接等领域中。

基于特征点的单目视觉位姿估计是经典的PnP(perspective-n-point)问题[3],即由透视N点来求解各点到投影中心的距离,该问题是在1981年首先由Fischler等人[4]提出,随后众多国内外学者给出了大量的PnP问题求解方法,这些方法一般可分为非迭代方法(闭式解法)和迭代方法(数值解法)2大类。

非迭代方法通过构造多项式方程组来解算相对位姿,通常适用于特征点数目较少的情况(如3～5个点),同时推导出了很多种闭式解法[5-6],也得出了位姿解的数量和唯一解条件等重要结论。该类方法具有运算量小、实时性好等优点,但受图像误差影响较大且精度不高,而且对野值(错误的特征点对应)比较敏感,只可用于迭代方法的初始值计算。

虽然利用3～5个特征点就可以完成位姿估计,但很多学者还是希望利用更多数量特征点,通过引入冗余来克服图像噪声和野值的影响,提高位姿解的精度。一般做法是采用多点迭代方法[7-9]求解PnP问题,将其表示为受约束的非线性优化问题。该类方法通常适用于特征点数目较多的情况,而且对噪声和野值具有较好的鲁棒性,其思路一般是首先构造目标函数,然后利用优化理论最小化目标函数得出位姿解。

1 位姿估计

1.1 问题陈述

已知地面控制点p,其世界坐标为(x,y,z), p点在相片上的投影点为p′,假设相机已经进行了内标定,即相机的内参数和畸变系数已知,由像素坐标和相机内参数,通过内定向可以得到p′在相机坐标系下的坐标(x′,y′,z′)。称p和p′为1个样本点对。

位姿估计是在已知若干样本点对后,求解相机坐标系相对于世界坐标系的位置和姿态的过程。如图1所示,x、y、z和x′、y′、z′分别为世界坐标系和相机坐标系3轴,ABCD为成像平面,空间3点p1、p2、p3对应的像点为p′1、p′2、p′3, 3个空间点的投影射线向量分别为、,记对应的单位向量为V1、V2、V3,有

3个向量对应的夹角分别为α、β、γ,则cosα= V1·V2,同理可得cosβ、cosγ。

由余弦定理有：

式中：d1、d2、d3为3个空间点相互之间的距离,可以由空间点的坐标求得;l1、l2、l3为投影中心o′到空间点的距离。

图1 位姿估计示意图

根据空间点、像点和投影中心间的共线关系有

式中：R、T分别为旋转矩阵和平移向量。若已知l1、l2、l3,则由式(2)来确定R、T是一个绝对定向的过程。因此,位姿估计的关键是确定参数l1、l2、l3。

遵循一般的求解流程,本文首先采用一种三点线性迭代算法求解各空间点到投影中心的距离,然后通过绝对定向得到相机位姿初始值;以此为基础,根据共线约束,以最小化像平面重投影误差为优化目标,通过非线性迭代优化来获取位姿的最优解。

1.2 三点迭代算法

采用一种线性迭代的方法来求解各控制点到投影中心的距离li。此种方法只需3个控制点,不涉及线性方程组的求解,解算速度快。

已知3个样本点对,可以得到如式(1)所示的3个方程。改写方程组中的方程,以第1式为例,因为,因此,第1式可写为

令

代入式(5),从而可将式(1)改写成如下的形式

将此方程组写成矩阵形式

由于

代入式(6),则可得到求解空间点到投影中心距离li的迭代计算公式

令

式中：l(k)1、l(k)2、l(k)3、n(k)、x(k)为各变量第k次迭代后的结果。

这样,迭代计算公式可写成如下形式

在视觉测量领域,控制点相互之间到投影中心的距离差相比于到投影中心的绝对距离一般都较小。利用这一特点,可假设初始迭代时,即当k=0时,l1≈l2≈l3,从而有：

有了上面的初始条件,就可以根据式(7)对待求的li进行迭代解算。当为某一预设的门槛)时,迭代结束。测试表明,迭代5次左右即可结束。

相比文献[10]中提到的方法,此种方法更为简单,高效。值得注意的是,若3个样本点共线或近似共线,则迭代过程不收敛,一般的做法是从所有样本点中取3点,保证3点在图像平面上所成三角形面积最大。

1.3 绝对定向

在获取了各空间点到投影中心的距离后,根据式(2)求解R、T就是一个绝对定向问题,即确定2个空间点集间的旋转、平移、比例变换关系,优化目标可表示为

式中：λ为比例系数。常用的绝对定向方法有四元数法[11-12]和SVD法[13-14]等。

此处的2个点集已消除了比例变换,即λ= 1。在此基础上,本文通过分解旋转矩阵,根据优化目标构造线性约束方程,采用线性最小二乘优化方法求解各未知量。

由于旋转矩阵是正交矩阵,因此R可用反对称矩阵表示为

设p′i=liVi=(x′iy′iz′i),pi=(xiyizi), T=(t1t2t3),因此式(2)可写为

将S代入式(9),并按已知量(pi、p′)和未知量(a、b、c、t1、t2、t3)进行整理,得

引入辅助参数u、v、w,设：

代入式(10),并进行整理,得

一个样本点对对应一个式(9)所示的方程,进而得到3个式(11)中的方程,则N个样本点对可以构造包含3N个方程的约束方程组,运用线性最小二乘方法求解该方程组,可以得到6个未知数a、b、c、u、v、w的最优解。旋转矩阵由式(8)得到,保证所得矩阵为正交矩阵;平移向量元素t1、t2、t3可由式(12)得到

文献[11]和文献[13]的方法需首先将点集中心化来消除平移变换,然后再单独解算旋转变换参数;上述方法无须中心化,而是同时求解平移和旋转变换参数,计算量也相对较小。

如果采用1.2节的迭代方法,则可由3个样本点对解算l1、l2、l3,进而得到9个由式(11)所确定的约束方程。采用最小二乘法解该方程组,并结合式(12),即可求得相机位姿的初始值。三点迭代算法加上绝对定向,即构成了位姿估计的三点直接算法。

1.4 迭代优化

为了减小样本点坐标的随机误差对解算结果造成的影响,以共线方程为基础,最终得到误差函数为：

式中：FΔx、FΔy即为像点横、纵坐标的实际值与理论值的偏差;x、y为中心化的像点坐标;a1、a2、a3、b1、b2、b3、c1、c2、c3为旋转矩阵元素;t1、t2、t3为平移向量元素。

以最小化像平面重投影误差作为优化目标,即

根据非线性最小二乘理论可以求解该优化问题。通过将FΔx、FΔy一阶泰勒展开,可将其转化为线性最小二乘问题。以各样本点对根据三点直接算法求得的位姿初值为基础,根据线性化后的式(13)构造约束方程组,用线性最小二乘方法解该方程组得到位姿校正量,以此更新位姿参数;多次迭代直至校正量足够小,从而得到位姿的最优解。

2 实验结果

本文采用仿真数据和实际数据2种方法来验证算法的有效性。

2.1 仿真实验

仿真数据生成：在坐标范围为x∈[-1 000, 1 000],y∈[-1 000,1 000],z∈[450,500]的立方体内产生10 000个随机分布的三维点;随机生成1 000个相机的姿态角及其空间位置,其中3个姿态角限定在[-10°,10°]范围内,空间位置限定在x∈[-200,200],y∈[-200,200],z∈[-10, 10]的范围内,以保证在相机视野内有足够多的空间点,并使视野内的空间点到投影中心的距离相差不大;相机焦距设为6 000像素,分辨率设为4 000像素×3 000像素,由以上数据可以得到每个相机的投影矩阵。根据投影矩阵可以获得每个在相机视野内的空间点所对应的像点坐标,并在横纵方向对像点坐标均分别加上均值为0、标准差为σ的高斯随机噪声,σ的取值在一定范围内(小于10),否则即为外点。从中随机取N个点(N≥3,根据实验需要确定N)来进行位姿解算。

相机位置误差通过公式d=‖Ti-T‖/‖T‖来衡量,其中Ti、T分别为解算的相机位置和理想的相机位置;相机姿态误差通过计算矩阵RiRT对应的3个欧拉角构成的向量的模来衡量,即：r=‖dω dφ dκ‖,Ri、R分别为解算的相机姿态角对应的旋转矩阵和理想姿态角对应的旋转矩阵。

图2为σ=0,1,…,10时,采用本文提出的三点直接算法及DLT(direct linear transform)算法解算位姿的结果,对于三点直接算法和DLT算法,N分别为3和6,即为2种算法最少所需的点数(已经剔除了N=6时控制点近似共面的情况,从而使DLT算法适用)。图2(a)是位置误差曲线,图2(b)是旋转误差曲线。每个σ处的误差均是用1 000个相机进行计算所得的平均结果。从图2中曲线可知,本文算法的旋转误差与DLT算法相近,但位置误差要优于DLT算法,此外本文算法只需3个样本点对,而DLT则需要6个样本点对才能解算。

图2 位姿解算结果

图3 为N=15,σ=0,1,…,10时,只采用三点直接算法(此时只需3个点)和三点直接算法加上基于共线约束的非线性迭代优化后的解算结果。图3(a)是位置误差曲线,图3(b)是旋转误差曲线。每个σ处的误差是用1 000个相机进行计算所得的平均结果。从图3中可以看出,加上非线性优化后,解算精度显著提高,对随机误差的抗性显著增强。

图4为σ=1.5时,随N的增加,采用三点直接算法及直接算法与迭代优化相结合的方法解算位姿的结果。

从图4中可以看出,当N＞7,随样本点数的增加,解算精度并没有显著提高。所以直接与迭代结合方法对样本点数不是很敏感,一般N＞7即可达到较高的精度;此外,三点直接算法随N的增加,精度也有所改善,这是由于在实现三点算法时,采用的是从N个样本点中取3个点,并保证3点所成三角形面积最大,即保证3个点分布范围尽量广,从而有效改善了三点算法的解算精度;从这一结果也可以看出,对于三点算法,3点分布越广,越有利于提高解算的精度。

图3 位姿解算结果

图4 位姿解算结果

由以上数据可知,本文方法仅需3点即可达到较高的解算精度,可将其与RANSAC(Random sample consensus)算法结合。对于有少量明显误点的样本点集,从点集中随机选取不共线的3点。利用直接解算与迭代优化的方法求解位姿,并根据其他样本点对的像平面重投影误差来判断解算结果的正确性：若误差在合理范围内的样本数达到一定比例,则认为是合理解;多次重复上述过程,并选取均方重投影误差最小的合理解作为最终的结果,同时剔除重投影误差较大的样本点对。

上述解算过程使算法对外点具有很强的抗性,保证了算法的鲁棒性。

2.2 实际数据实验

采用威海天福山一块面积为2 km2的试验场,场内布有179个基本控制点。从中选取20个控制点作为测试点集,其余的作为训练集。用飞机对场区进行航拍,得到了5条航线、共115幅航空影像;所用相机为哈苏H4D-60,镜头焦距为50 mm,分辨率为8 956像素×6 708像素。影像的航向重叠率约为60%,旁向重叠率大于20%,影像空间分辨率约为0.05 m/像素,其中有107幅影像上分布了不少于3个的训练控制点(很多影像上仅有3个控制点),某些区域由于树木的遮挡,控制点无法清晰辨识;通过标定实验得到了相机的内参数和畸变系数。

人工拾取每幅影像中控制点的相片坐标,利用上述直接与迭代结合的方法,解算每幅有效影像对应的相机位姿。以这些位姿为基础,利用同名射线相交法(空间前方交会)反算测试点集中控制点的空间坐标,表1是解算结果与真实结果间的误差比较。

从表1数据中可以看出,大部分点的解算误差都在0.1 m(2个像素)以内,可见解算的各相机方位参数精度较高;个别点(如162)距离误差较大(0.830),这是因为引起误差的原因不仅包括相机位姿解算的误差,还包括相机标定误差、控制点像坐标拾取误差及控制点空间坐标的量测误差等。

以解算的相机位姿为基础,通过提取相邻航片的同名像点,并利用射线相交法解算每对同名点的空间坐标,进而得到覆盖全场区的空间点云,对点云进行三角构网、纹理映射,得到场区的三维地形,如图5所示。图5(a)是三角构网后的地形图,图5(b)是纹理映射后的三维景观图。

从图5中可看到高度真实感的三维场景,这些都得益于高精度的相机位姿参数,从而进一步证明了所提算法的有效性。

表1 测试点集空间坐标解算误差 m

图5 三维重建结果

3 结 论

本文所提位姿估计方法精度高、鲁棒性高、计算量小,最少只需3个控制点即可进行解算。由于所需的点数少,所以算法对外点(误差很大的点)的抗性很强,通过与RANSAC算法结合,可以有效地消除少量外点的影响。所提直接最小二乘绝对定向方法解算精度高,可以达到直接与迭代相结合方法同等的精度。在摄影测量领域,布设、测量地面控制点费时费力,而此算法只需3个控制点即可解算,减少了外业工作量。传统的DLT算法对于平面结构(控制点共面)不适用,并且需要6个控制点才能进行解算,与之相比,本文算法具有更广泛的适用性。不足之处是,此方法只适用于3个控制点到投影中心距离相差不大的情况,航空摄影测量中一般都满足这一条件。对于近景摄影测量,若控制点在景深方向的分布范围太广,则此算法不再适用。

References)

[1]黄邦奎,刘震,张文军.多传感器线结构光视觉测量系统全局校准[J].光电子·激光,2011,22(12)：1816-1820.

[2]肖永亮,苏显渝,薛俊鹏,等.基于凸松弛全局优化算法的视觉测量位姿估计[J].光电子·激光,2011,22(9)：1384-1389.

[3]HARTLEY R I,ZISSERMAN A.Multiple view geometry in computer vision[M].Cambridge：Cambridge University Press,2004：121-132.

[4]FISCHLER M A,BOLLES R C.Random sample consensus：a paradigm for model fitting with applications to image analysis and automated cartography[J].Communications ACM, 1981,24(6)：381-395.

[5]KNEIP L,SCARAMUZZA D,SIEGWART R.A novel parameterization of the perspective-three-point problem for a direct computation of absolute camera position and orientation[C]//Computer Vision and Pattern Recognition (CVPR).2011 IEEE Conference on.Providence.RI：IEEE, 2011：2969-2976.

[6]KUKELOVA Z,BUJNAK M,PAJDLA T.Closed-form solutions to the minimal absolute pose problems with known vertical direction[C]//Computer Vision-ACCV 2010.Berlin Heidelberg：Springer,2011：216-229.

[7]VINCENT L,FRANCESC M N,PASCAL F.EPnP：an accurate O(n)solution to the PnP problem[J].International Journal of Computer Vision,2009,81(2)：155-166.

[8]HESCH J A,ROUMELIOTIS S I.A direct least-squares (DIS)method for PnP[C]//Computer Vision(ICCV).2011 IEEE International Conference on.Barcelona：IEEE,2011：383-390.

[9]OLSSON C,KAHL F,OSKARSSON M.Optimal estimation of perspective camera pose[C]//ICPR.18thInternational Conference on.Hong Kong：IEEE,2006：5-8.

[10]QUAN L,LAN Z.Linear N-point camera pose determination[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1999,21(8)：774-780.

[11]HORN B K P.Closed-form solution of absolute orientation using unit quaternion[J].J.Optical Soc.Am.,1987,4：629-642.

[12]WALKER M W,SHAO L,VOLZ R A.Estimating 3D location parameters using dual number quaternion[J].CVGIP：Image Understanding,1991,54(3)：358-367.

[13]HORN B K P,HILDEN H M,NEGAHDARIPOUR S. Closed-Form solution of absolute orientation using orthonomal matrices[J].J.Optical Soc.Am.,1988,5：1127-1135.

[14]ARUN K S,HUANG T S,BLOSTEIN S D.A Leastsquares fitting of two 3D point sets[J].IEEE Trans.Pattern Analysis and Machine Intelligence,1987,9：698-700.

(编辑：孙陆青)

A Robust Method for Camera Pose Estimation

YANG Ahua1, LI Xuejun2, LIU Tao2, LIU Chunxiao3
(1.Department of Graduate Management,Equipment Academy,Beijing 101416,China; 2.Department of Information Equipment,Equipment Academy,Beijing 101416,China; 3.Military Representative Office of Chongqing Beibei District,Chongqing 400700,China)

Against the requirement of robustness and accuracy of pose estimation,a robust and accurate pose estimation method is proposed.The method is based on 3 sample point pairs.First,the distance of every spatial point to the projection center is assumed to be nearly the same.Then the equation group,from which the iteration formula is derived,is constructed according to the geometry constraint relationship between the image points and the spatial points.Subsequently,the distance between the spatial points and the projection center is solved iteratively.Based on the above result,a least square absolute orientation method is proposed to solve the initial pose of the camera.Finally, the initial camera pose is optimized iteratively based on the colinearity constraint between the spatial points and the image points.Experiment results illustrate that,in the assumption condition,the proposed method can converge well and achieve high accuracy.

pose estimation;sample point pair;absolute orientation;colinearity constraint

TP 391.41

2095-3828(2014)01-0088-07

ADOI10.3783/j.issn.2095-3828.2014.01.020

2013-05-22

杨阿华(1985-),男,博士研究生.主要研究方向：计算机图形,图像处理.李学军,男,教授,博士生导师.