一种基于直线特征的单目视觉位姿测量方法

1、光 电 子 激 光journal of optoelectronics laser第 21 卷 第 6 期 2010 年 6 月vol . 21 no . 6 j un. 2010一种基于直线特征的单目视觉位姿测量方法3赵汝进1 ,2 3 3, 张启衡1 , 左颢睿1 ,2 , 吴明军1 ,2(1. 中国科学院 光电技术研究所 ,四川成都 610209 ; 2. 中国科学院 研究生院 ,北京 100039)摘要 :提出了一种基于直线特征的单目视觉位姿测量方法 soft newton 。构造了新颖的目标直线与图像直线匹配评价函数 ,避免检测图像中直线的端点 ,最终通过软决策技术确定直线特征匹配关

2、系 ,并采用高斯牛顿迭代 算法基于全透视成像模型解算目标位姿。和 posit 算法相比 ,高斯牛顿迭代算法保持了旋转矩阵的正交性 , 提高位姿解算精度。仿真图像实验中 ,在干扰直线和噪声存在的情况下算法经过 29 次迭代解算得到正确的直 线特征匹配矩阵 ,姿态误差小于 0. 2,位移误差小于 0. 5 mm。仿真图像和实际图像实验结果均表明 soft new2 ton 具有较高解算精度和较强的鲁棒性。关键词 :单目视觉 ; 位姿; 软决策 ; 高斯牛顿法中图分类号 : tp39文献标识码 :a文章编号 :100520086 (2010) 0620894204a mono2vision meth

3、od of measuring pose based on line featureszhao ru2jin1 ,2 3 3 , zhan g qi2heng1 , zuo hao2rui1 ,2 , wu ming2jun1 ,2(1. instit ute of optics and elect ronics ,chinese academy of sciences ,chengdu 610209 ,china ; 2. graduate school of t he chinese academy of sciences ,beijing 100039 ,china)abstract :

4、a new mono2vision algorit hm to measure pose based on line feat ures , called soft newton ,is proposed. the new evaluating f unction for t he matches bet ween image lines and object lines is const ruc2 ted ,which avoids detecting t he lines endpoint s in image. the algorit hm applies soft2assign tec

5、hnique to determining t he correspondence of lines feat ure bet ween image and object . fut hermore , gauss2newton iterative algorit hm ,t hat computes object pose under a f ull2perspective camera model ,keep s t he rotation mat rix ort hogonal and get s higher precise result s. experimentally ,t he

6、 simulated images wit h dist urbing lines and noise are measured by soft newton. after 29 times of iteration ,t he accurate match mat rix oflines feat ure is achieved. as a result ,t he attit ude error is less t han 0. 2,and t he position error is lesst han 0. 5 mm. experiment s involving synt hesis

7、 images as well as real images demonst rates t he robust2ness and accuracy of soft newton.key words :mono2vision ; pose ; soft2assign ; gauss2newton algorit hm法结合形成 soft posit 算法 ,同时确定目标特征点与图像特征1引言单目视觉位姿测量 1 包括位姿解算和特征匹配两方面。 在目标特征和图像特征完全匹配前提下 ,目前已提出许多解算 目标空间位姿的成熟方法 25 。已在目标位姿确定前提下 ,特 征匹配通过将目标重投影与原图像进

8、行图匹配 6 、hausdorff 距离匹配 7 等方法解决。而实际工程应用中 ,单目视觉位姿测 量难点在于必须同时解决特征匹配和位姿解算两方面问题。 fishler 等人 8 提出的基于假设检验策略的 ransac 方法能够 保证特征匹配的鲁棒性 ,在单目视觉图像匹配 3 以及立体视觉 图像匹配 9 中均有广泛的应用。鉴于 ransac 庞大运算量 , david 等人 10 ,11 将软决策技术( soft2assign) 12 ,13 和 posit 4 ,5 算点的匹配关系 ,并基于正交尺度投影(弱透视) 解算目标位姿。因为图像中直线特征相对于点特征更加丰富而稳定 ,具有抗干扰和遮挡等

9、优点 ,david 等人 4 又将 soft posit 算法扩展应用 于直线特征。本文在 soft posit 基础上提出了基于直线特征的 soft2newton 方法 ,利用 soft2assign 确定图像直线和目标直线特征 匹配关系 ,同时 soft newton 改进了目标函数中评价直线特征 匹配的方法 ,避免检测图像中直线端点 ,并且利用高斯牛顿 ( gauss2newton) 迭代算法 2 ,3 基于全透视成像模型解算目标位 姿 ,保持了目标旋转矩阵的正交性 ,使最终位姿解具有更高精 度。收稿日期 :2009209204 修订日期 :20092112183 基金项目 :国家“86

10、3”计划资助项目(2003aa823050)3 3 e2mail :zrjo515 163. com线段端点距离加权和作为直线匹配评价函数 ,该函数要求检测出图像直线的端点 ,而当直线出现断裂或者图像噪声干扰下 ,往往直线端点不易准确获取。为此 ,本文通过cj ,cj 到 li 的距 离和构造 lj 与 li 匹配评价函数2坐标系定义和相机模型首先 ,以相机光心为原点建立相机坐标系 oc xc yc zc , oc zc 与 相机主光轴重合; op uv 为 2 维图像坐标系 , u、v 表示图像行和 列 ,且分别平行于 oc yc 和 oc zc 。在目标上 ,固联原点为 ot 的目标 坐标

11、系 ot xt yt zt ,如图 1 所示 ,位姿测量目标是解算目标坐标系 和相机坐标系之间的相对位移 t 和姿态 r 。其中 t = ( t1 , t2 , t3 ) t 为平移向量表示目标在相机坐标系下空间位移 , r 为旋转 矩阵由目标饶3 坐标轴的旋转角( pitch , yaw , roll ) 表示。目标特征 直线 l j 在一定位姿下像平面(image plane) 投影成像为 lj 。cj , li + cj , li 22disij=( )4图 1 中 ,当 disij 越大 , 说明 lj 与 li 匹配性越差 , (, t) 越偏离真解;反之 disij 越小, lj

12、与 li 一致性越好 , 此时(, t) 越接近 真解 ,当 disij 为零时, lj 与 li 完全重合。式(4) 在避免检测图像 中线段端点情况下 ,准确建立了 lj 与 li 的匹配评价函数。由于图像受噪声、背景变化等因素干扰 ,显然不一定每一条检测得到的图像直线都能找到与之匹配的目标直线 ,又或 者 ,不一定每一条目标直线都能找到与之匹配的图像直线。为 了描述检测获取的图像直线集与已知的目标直线集匹配关系 , 设置匹配矩阵 m , m 中沿竖直方向表示图像直线, 沿水平方向为目标直线。已知目标直线集 l j ,1 j j ,检测得到的图像 直线集 li ,1 i i 。mi , j

13、为 m 中第 i 行 j 列元素 , 表示第 i 条图像直线 li 和第 j 条目标直线 l j 匹配性 , mi , j 越大说明匹配程度越高。且 mi , j 满足mi , j 0 ,1 ,1 i i + 1 ,1 j j + 1j +1j = 1 mi , j = 1 ,1 i i(5)i +1i = 1 mi , j = 1 ,1 j j若 mi + 1 , j = 1 ,表示目标直线 l j 找不到与之匹配的图像直 线;而 mi , j + 1 = 1 , 表示图像直线 li 找不到与之匹配的目标直线。目标直线 l j 和图像直线 lj 除了满足式(3) 的几何约束 , 其匹配矩阵

14、m 还受到式 ( 5) 约束。为此 , 利用 mi , j 加权后的 disij 函数,构造最优化目标函数图 1 坐标系定义与直线匹配几何示意图fig. 1 the def inition of reference frame and the geometry of line correspondences假设某一目标特征点 p 在目标坐标系下坐标为 c = ( x , y ,z) t ,点 p 在相机坐标系下坐标为 ccm = ( x, y, z) t , c 和 ccm 变 换关系为ij( )m , , t = argmin e = argmin mi , j disij( )6i = 1

15、 j = 13. 2 高斯牛顿解算位姿在 m 确定情况下 ,采用高斯牛顿法迭代求解式(6) 非线性 问题。利用高斯牛顿法在全透视成像模型下解算目标位姿 ,保持了 r () 的正交性 , 使(, t) 具有更好精度; 其次高斯牛顿法并非直接求解(, t) 最优解 , 而是通过求解(, t) 的修正量,不断逼近最优(, t) 解 ,有ccm= r() c + t(1)式中 : r() = ( rt , rt , rt ) 为旋转矩阵; f 、f 分别为图像 u、v 方1 2 3u v向的尺度因子(由相机焦距和像元尺寸决定) ; ( uo , vo ) 为图像中心坐标。则 p 投影到图像平面坐标 c

16、 为rt1 + c + t1f u + uort3 c + t3c = f( c) =(2)(, t) k+1 = (, t) ( k) - (7)rt2 + c + t2f v + vo( ), t ( k)表示第 k 次迭代解;通过式中 :rt3 c + t3j ac =e(8)(9)3基于特征直线的位姿测量原理 = ( j t jac ) - 1 j t eacac迭代求解。其中 , jac = 5 e/ 5 (, t) , 为雅可比矩阵。在给定合理的位姿初始估计(, t) 0 条件下 ,随着 e 在迭代过程中逐渐收敛 到最小 , (, t) ( k) 将不断逼近最优解。3. 3 sof

17、t2assign 求解匹配矩阵为使式(5) 中 m 元素 mi , j 最终收敛并逼近到 0 或者 1 (0 表 示不匹配 ,1 表示匹配) ,采用 soft2assign 策略 10 ,11 迭代寻求 m最优解。soft2assign 由 sinkhorn 15 和确定性退火(deterministicannealing) 技术 16 的交替迭代组成 ,其中 sinkhorn 算法改进后 对 m 进行归一化,确保每次迭代后 m 每行或每列的和为 1 ,且 每行、每列元素最大值与 i + 1 行、j + 1 列元素的比值保持不变。而确定性退火技术随着迭代进行 ,使具有较小 disij 对应的3

18、. 1 目标函数建立式(2) 为经典摄影测量理论中的特征点共线性方程 ,而空 间中 2 个三维点坐标即可完全确定三维目标直线 ,即用目标坐标系下两点 cj , cj 表示第 j 条目标直线 l j 。假设目标直线l j 对应第 i 条图像直线 li ,而 l j 以位姿(, t) 投影到图像上为lj ,即 lj 可由目标点 cj , cj 在图像上投影点cj ,cj 表示为cj ,cj = f( cj ) , f( cj ) (3)在(, t) 估计不准确情况下 , lj 与实际图像直线 lj 不完全一致。反之 ,若 lj 与 li 完全重合 ,此时(, t) 估计即为真实位姿 解。为了衡量

19、lj 与 lj 的一致性,文献14 中利用两直线夹角和光 电 子激 光 2010 年 第 21 卷896 mij 在 sinkhorn 归一化后逐渐收敛逼近 1 ,而较大 disij 对应的mi , j 逐渐收敛逼近 0 ,最终确立 l j 和 li 匹配关系。m 在退火开始前首先设置 i + 1 行、j + 1 列元素为较小常 数 ,而其余元素满足soft newton 在干扰直线和噪声存在情况下依然能够将图像直线集和目标直线集进行正确匹配 ,且如表 1 所示 ,仿真实验结 果姿态测量值和姿态真值误差在 0. 2以内 ,且位移误差小于 0.5 mm 。mi , j = exp ( - ( d

20、isij - ) )(10)参数控制退火温度, 初始设置为较小值, 随着迭代的进行 ,= ,其中 为大于 1 的常数 ,则在迭代过程中逐渐 增大。参数决定 disij 最大误差,即当两直线 disij 时 ,则认 为 li 和 l j 不匹配 ,所以应该是图像的噪声级别的函数。假 设图像噪声符合均值为 0 、标准差为的正态分布, 则设置=102 。由于初始 (, t) 0 估计不准确, 所以正确对应关系下的disij 亦会大于,然而初始设置较小,所以算法在初始阶段将 考虑所有匹配的可能性 , 随着迭代进行 ,逐渐增大, 正确对应mi , j 下的位姿也逐步逼近真值。在迭代后期 , disij

21、对应的mi , j 将被认为错误对应 ,逐步收敛到 0 。最终 soft2assign 和高斯牛顿法结合形成 soft newton 算法 ,m 与(, t) 迭代求解过程如下 :图 2fig. 2图像直线与立方体边缘重投影成像结果image lines and the projection of cube edge1) 设置参数、初始化(, t) 0 、m 和 l , 检测图像中直j线 li ;2) 将 l j 中 cj , cj 按式(2) 投影到图像平面 , 并根据式(4) 计算 disij ;3) 由式(10) 计算 mi , j , 并利用 sinkhorn 算法对 mi , j 归

22、一化 处理;4) 在得到 mi , j 基础上,式(7) (9) 迭代解算位姿(, t) ;5) 若lim ,则停止迭代 ,否则设置= , 并返回第 2步继续运算。4实验结果为了验证算法的正确性和有效性 ,利用仿真图像和真实图 像对 soft newton 进行测试验证。仿真图像生成过程为 :1) 立方体目标尺寸为 50 mm 50 mm 50 mm ,图像大小为 600 pixel 600 pixel ,相机内参 f u = f v = 1730 , uo = vo = 300 ;立方体 12 条边设为目标直线,按一定姿态投影到图像平面;2) 将生成图像直线端点坐标均加上均值为 0 、方差为

23、 2 的 高斯正态噪声;3) 图像由于背景变化 , 检测到图像直线有可能并非目标直线的投影 ,为此在图像中随机添加 19 条干扰直线。图 2 中 ,虚线表示已知图像直线共 31 条(12 条真实目标边 缘投影和 19 条干扰直线) , 实线则表示目标以当前(, t) 重新 投影成像结果。其中 : (a) 为目标以初始估计(, t) 0 投影 ;而经 过 5 、9 、17 、25 和 29 次迭代解算(, t) 后 ,目标重投影结果如(b)- (f) 实线所示。在迭代过程中 ,解算结果逐渐向真实位姿逼近 ,经过 29 次迭代后解算得到正确位姿。如(f) 所示 ,最终目标 重投影结果和原图像中目标

24、边缘投影重合。soft newton 迭代过程中 ,匹配矩阵 m 演化情况如图 3 所示 ,其中亮度越高的像素对应 m 中元素 mi , j 值越大。仿真实验 设置前 12 条图像直线分别对应第 1 - 12 条目标直线 , (a) 表示初始 m , (b) - (f) 则为经过 5 、9 、17 、25 和 29 次迭代后的 m 。经 过 29 次迭代运算获得正确的匹配矩阵 , 矩阵最后一列元素表示干扰直线未找到任何与之匹配的目标直线。实验结果表明 ,图 3 匹配矩阵演化fig. 3 the evolution of matching matrix应用 soft newton 对 l 型工件

25、位姿进行自动解算 ,如图 4所示。首先利用 sobel 检测图像边缘 ,并利用 hough 变换检测图像中直线 ,如(a) 所示 ,检测得到符合要求的共 18 条直线。其中仅有 7 条为真实目标边缘直线投影 ,其余 11 条不属于目标边缘投影 ,为干扰直线。而目标其余边缘投影也未被检测获 取。l 型工件以初始估计位姿重投影成像结果如( b) 所示 ,此时 ,目标初始姿态与目标真实姿态相差约 10。经过 soft new2ton 解算位姿结果如(c) 所示 ,l 型工件模型以解算得到的位姿 重投影成像与目标实际成像边缘相互重合 ,表明 soft newton 算法位姿解算的正确性和鲁棒性。 5

26、gra me gna t ,venturino l ,cicirelli g. optimization of the positalgorithm for indoor autono mous navigation j . ro botic s andautono mous syst e ms ,2004 ,48 (223) :1452162 .cont e d , fo ggia p , sansone c , et al . thirty ye ars of grap h mat ching in p att ern re co gnition j . int ernational j

27、ournal of pat2t ern re co gnition and artificial int elligence , 2004 , 18 ( 3 ) : 2652298 .zhao c , shi w , deng y. a new hausdorff di st ance for ima ge mat ching j . patt ern re co gnition lett ers ,2005 ,26 :5812586 .fi schler m a ,bolle s r c. rando m sa mple consensus :a p ara2digm for mo del

28、fitting with applications to ima ge ,analysi s andauto mat e d carto graphy j . co mmunications of the acm ,1981 ,24 (6) :3812395 .ro bert cup e c , andreja kit anov , ivan petrovic . st ereo ima ge re gi stration ba se d on ransac with selective hypothe si s gen2 eration a . e dit e d by lui s ba s

29、anz ,raulsuarez and j an ro sell . pro ce e dings boo k of 40th int ernational sympo sium on ro botic s c . 2009 ,2652270 .david p ,dement hon d f ,durai swa mi r ,et al . soft posit : sim2 ult aneous po se and corre spondence det ermination j . int erna2 tional j ournal of co mput er vi sion ,2004

30、,59 (3) :2592284 .david p ,dement hon d f ,durai swa mi r ,et al . soft posit : sim2 ult aneous po se and corre spondence det ermination a . pro2 ce e dings of the 7th europ e an conference on co mput er vi sion2part iii c . 2002 ,6982714 .miguel angel lozano and franci sco escolano . a significant

31、im2 pro ve ment of soft a ssign with diffusion kernel s j . structural , synt actic ,and st ati stical patt ern re co gnition , 2004 , 3138 : 76284 . 6 表 1 仿真图像位姿解算结果tab. 1 the pose result of simulated image 7 ()t/ mminitiale sti mat erealpo se( - 5 ,50 , - 110)( - 13 ,40 , - 100)( 8 , - 12 ,750)( 0

32、 ,0 ,600) 8 ( - 12 . 884 3 ,39 . 980 6 ,( 0 . 241 2 ,0 . 178 6 ,re sult- 100 . 184 8)600 . 474 3) 9 10 图 4 应用 sof t ne wton 对 l 型工件位姿进行自动解算fig. 4 sof t ne wton is applied to computing the pose of the l prof ile workpiece 11 12 5 结论基于 soft2assign 策略确定目标直线与图像直线的匹配关 系 ,利用高斯牛顿迭代方法解算全透视下目标位姿。仿真图像 实验中 ,在噪

33、声和干扰直线的情况下 ,经过 29 次迭代解算得到 正确的直线特征匹配矩阵 ,姿态测量结果误差小于 0. 2,位移 误差小于 0. 5 mm 。仿真图像和实际图像实验结果表明 ,soft2newton 具有较高解算精度和较强的鲁棒性。 13 brijne sh j j ain ,micha el lapp e . j oining soft a ssign and dyna micpro gra mming for the cont act map overlap pro ble m j . bioin2formatic s re se arch and de velop ment ,2007

34、 ,4414 :4102423 . 14 david p , dementhon d f , durai swa mi r , et al . simult aneouspo se and corre spondence det ermination using line fe ature s a . proce e dings of t he 2003 ieee co mput er society confer2 ence on co mput er vi sion and patt ern re co gnition ( cvpr03 ) c . 2003 ,2 :4242431 .ra

35、ngarajan a , yuille a ,mjolsne ss e. convergence prop ertie s of the soft a ssign qua dratic a ssignment algorit hm j . neural co m2 put ation ,1999 ,11 (6) :145521474 .vika s sindhwani , sathiya ke ert hi , olivier chap elle . det ermin2i stic anne aling for se mi2sup ervi se d kernel ma chine s a . pro2 ce e dings of the 23rd int ernational conference on ma chine le arning t a ble of cont e st s c . 2006 ,8412848 .zhu shi2ping ,xia xi , zhang qing2rong. an e dge det e ction al2gori