一種基于直線特征的單目視覺位姿測量方法

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 。構造了新穎的目標直線與圖像直線匹配評價函數(shù) ,避免檢測圖像中直線的端點 ,最終通過軟決策技術確定直線特征匹配關

2、系 ,并采用高斯牛頓迭代 算法基于全透視成像模型解算目標位姿。和 posit 算法相比 ,高斯牛頓迭代算法保持了旋轉(zhuǎn)矩陣的正交性 , 提高位姿解算精度。仿真圖像實驗中 ,在干擾直線和噪聲存在的情況下算法經(jīng)過 29 次迭代解算得到正確的直 線特征匹配矩陣 ,姿態(tài)誤差小于 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 算點的匹配關系 ,并基于正交尺度投影(弱透視) 解算目標位姿。因為圖像中直線特征相對于點特征更加豐富而穩(wěn)定 ,具有抗干擾和遮擋等

9、優(yōu)點 ,david 等人 4 又將 soft posit 算法擴展應用 于直線特征。本文在 soft posit 基礎上提出了基于直線特征的 soft2newton 方法 ,利用 soft2assign 確定圖像直線和目標直線特征 匹配關系 ,同時 soft newton 改進了目標函數(shù)中評價直線特征 匹配的方法 ,避免檢測圖像中直線端點 ,并且利用高斯牛頓 ( gauss2newton) 迭代算法 2 ,3 基于全透視成像模型解算目標位 姿 ,保持了目標旋轉(zhuǎn)矩陣的正交性 ,使最終位姿解具有更高精 度。收稿日期 :2009209204 修訂日期 :20092112183 基金項目 :國家“86

10、3”計劃資助項目(2003aa823050)3 3 e2mail :zrjo515 163. com線段端點距離加權和作為直線匹配評價函數(shù) ,該函數(shù)要求檢測出圖像直線的端點 ,而當直線出現(xiàn)斷裂或者圖像噪聲干擾下 ,往往直線端點不易準確獲取。為此 ,本文通過cj ,cj 到 li 的距 離和構造 lj 與 li 匹配評價函數(shù)2坐標系定義和相機模型首先 ,以相機光心為原點建立相機坐標系 oc xc yc zc , oc zc 與 相機主光軸重合; op uv 為 2 維圖像坐標系 , u、v 表示圖像行和 列 ,且分別平行于 oc yc 和 oc zc 。在目標上 ,固聯(lián)原點為 ot 的目標 坐標

11、系 ot xt yt zt ,如圖 1 所示 ,位姿測量目標是解算目標坐標系 和相機坐標系之間的相對位移 t 和姿態(tài) r 。其中 t = ( t1 , t2 , t3 ) t 為平移向量表示目標在相機坐標系下空間位移 , r 為旋轉(zhuǎn) 矩陣由目標饒3 坐標軸的旋轉(zhuǎn)角( 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 的匹配評價函數(shù)。由于圖像受噪聲、背景變化等因素干擾 ,顯然不一定每一條檢測得到的圖像直線都能找到與之匹配的目標直線 ,又或 者 ,不一定每一條目標直線都能找到與之匹配的圖像直線。為 了描述檢測獲取的圖像直線集與已知的目標直線集匹配關系 , 設置匹配矩陣 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 函數(shù),構造最優(yōu)化目標函數(shù)圖 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) 最優(yōu)解 , 而是通過求解(, t) 的修正量,不斷逼近最優(yōu)(, t) 解 ,有ccm= r() c + t(1)式中 : r() = ( rt , rt , rt ) 為旋轉(zhuǎn)矩陣; 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) 將不斷逼近最優(yōu)解。3. 3 sof

17、t2assign 求解匹配矩陣為使式(5) 中 m 元素 mi , j 最終收斂并逼近到 0 或者 1 (0 表 示不匹配 ,1 表示匹配) ,采用 soft2assign 策略 10 ,11 迭代尋求 m最優(yōu)解。soft2assign 由 sinkhorn 15 和確定性退火(deterministicannealing) 技術 16 的交替迭代組成 ,其中 sinkhorn 算法改進后 對 m 進行歸一化,確保每次迭代后 m 每行或每列的和為 1 ,且 每行、每列元素最大值與 i + 1 行、j + 1 列元素的比值保持不變。而確定性退火技術隨著迭代進行 ,使具有較小 disij 對應的3

18、. 1 目標函數(shù)建立式(2) 為經(jīng)典攝影測量理論中的特征點共線性方程 ,而空 間中 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 列元素為較小常 數(shù) ,而其余元素滿足soft newton 在干擾直線和噪聲存在情況下依然能夠?qū)D像直線集和目標直線集進行正確匹配 ,且如表 1 所示 ,仿真實驗結 果姿態(tài)測量值和姿態(tài)真值誤差在 0. 2以內(nèi) ,且位移誤差小于 0.5 mm 。mi , j = exp ( - ( d

20、isij - ) )(10)參數(shù)控制退火溫度, 初始設置為較小值, 隨著迭代的進行 ,= ,其中 為大于 1 的常數(shù) ,則在迭代過程中逐漸 增大。參數(shù)決定 disij 最大誤差,即當兩直線 disij 時 ,則認 為 li 和 l j 不匹配 ,所以應該是圖像的噪聲級別的函數(shù)。假 設圖像噪聲符合均值為 0 、標準差為的正態(tài)分布, 則設置=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) 設置參數(shù)、初始化(, t) 0 、m 和 l , 檢測圖像中直j線 li ;2) 將 l j 中 cj , cj 按式(2) 投影到圖像平面 , 并根據(jù)式(4) 計算 disij ;3) 由式(10) 計算 mi , j , 并利用 sinkhorn 算法對 mi , j 歸

22、一化 處理;4) 在得到 mi , j 基礎上,式(7) (9) 迭代解算位姿(, t) ;5) 若lim ,則停止迭代 ,否則設置= , 并返回第 2步繼續(xù)運算。4實驗結果為了驗證算法的正確性和有效性 ,利用仿真圖像和真實圖 像對 soft newton 進行測試驗證。仿真圖像生成過程為 :1) 立方體目標尺寸為 50 mm 50 mm 50 mm ,圖像大小為 600 pixel 600 pixel ,相機內(nèi)參 f u = f v = 1730 , uo = vo = 300 ;立方體 12 條邊設為目標直線,按一定姿態(tài)投影到圖像平面;2) 將生成圖像直線端點坐標均加上均值為 0 、方差為

23、 2 的 高斯正態(tài)噪聲;3) 圖像由于背景變化 , 檢測到圖像直線有可能并非目標直線的投影 ,為此在圖像中隨機添加 19 條干擾直線。圖 2 中 ,虛線表示已知圖像直線共 31 條(12 條真實目標邊 緣投影和 19 條干擾直線) , 實線則表示目標以當前(, t) 重新 投影成像結果。其中 : (a) 為目標以初始估計(, t) 0 投影 ;而經(jīng) 過 5 、9 、17 、25 和 29 次迭代解算(, t) 后 ,目標重投影結果如(b)- (f) 實線所示。在迭代過程中 ,解算結果逐漸向真實位姿逼近 ,經(jīng)過 29 次迭代后解算得到正確位姿。如(f) 所示 ,最終目標 重投影結果和原圖像中目標

24、邊緣投影重合。soft newton 迭代過程中 ,匹配矩陣 m 演化情況如圖 3 所示 ,其中亮度越高的像素對應 m 中元素 mi , j 值越大。仿真實驗 設置前 12 條圖像直線分別對應第 1 - 12 條目標直線 , (a) 表示初始 m , (b) - (f) 則為經(jīng)過 5 、9 、17 、25 和 29 次迭代后的 m 。經(jīng) 過 29 次迭代運算獲得正確的匹配矩陣 , 矩陣最后一列元素表示干擾直線未找到任何與之匹配的目標直線。實驗結果表明 ,圖 3 匹配矩陣演化fig. 3 the evolution of matching matrix應用 soft newton 對 l 型工件

25、位姿進行自動解算 ,如圖 4所示。首先利用 sobel 檢測圖像邊緣 ,并利用 hough 變換檢測圖像中直線 ,如(a) 所示 ,檢測得到符合要求的共 18 條直線。其中僅有 7 條為真實目標邊緣直線投影 ,其余 11 條不屬于目標邊緣投影 ,為干擾直線。而目標其余邊緣投影也未被檢測獲 取。l 型工件以初始估計位姿重投影成像結果如( b) 所示 ,此時 ,目標初始姿態(tài)與目標真實姿態(tài)相差約 10。經(jīng)過 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、聲和干擾直線的情況下 ,經(jīng)過 29 次迭代解算得到 正確的直線特征匹配矩陣 ,姿態(tài)測量結果誤差小于 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