EPnL: 一種高效且精確的PnL 問題求解算法

王平 何衛隆 張愛華 姚鵬鵬 徐貴力

基于直線特征的攝像機絕對位姿估計問題在計算機視覺領域稱之為Perspective-n-line (PnL)問題,目的是通過目標物體上的n條已知直線和其所對應的圖像投影來計算相機和目標之間的相對位置和姿態關系.PnL 問題是視覺導航[1?2]、機器人視覺定位[3]、傳感器標定[4]、現實增強[5]等領域中的關鍵核心問題之一.到目前為止,現有解決PnL 問題的方法可以粗略分為迭代法和非迭代法:

迭代法將PnL 求解問題轉換為非線性最優化問題,并利用Gauss-Newton 法或Levenberg-Marquardt法[6]迭代求解這個非線性最優化問題.然而,迭代法對初值的選取比較敏感,初值選擇不合理將導致迭代法收斂速度較慢,影響算法的實時性.除此之外,當使用特征直線較少的時候,迭代法容易陷入局部最優,影響PnL 問題求解的精度和可靠性.

在非迭代法中,最直接的當屬直接線性變換(Direct linear transformation,DLT)方法[7].DLT方法簡單高效,但抗噪能力不強,在空間參考直線較少的情況下求解精度不高.P?ibyl等[8]通過將直線在歐氏空間的坐標表示轉換到普呂克(Plücker)空間,提出了解決PnL 問題的DLT-Plücker-lines方法.相比于DLT 算法,DLT-Plücker-lines 算法具有更好的抗噪能力和求解精度,但其求解時需要至少9 條以上的空間參考直線.隨后,Xu等[9]通過借鑒Perspective-n-point (PnP)算法原理,提出了一系列線性求解PnL 的方法,但這些方法仍需要6條以上的空間參考直線才可以求解.最近,P?ibyl等[10]基于DLT 的方法,提出了DLT-combined-lines的方法,該方法將線性求解PnL 問題的直線數目縮減到了5 條.DLT-combined-lines 方法求解效率高,在直線數目較多的時候,具有很高的求解精度,但是在直線數目較少的情況下,求解精度較差.

雖然直接線性求解PnL 的方法具有簡單、效率高的特點.但是由于其抗噪能力差、不適合參考直線較少的情況(尤其不適合參考直線少于6 條以下的情況).為了克服這些問題,Ansar等[11]將PnL 問題轉換為非迭代優化 開發了一種通用的PnL 求解算法,該算法能夠處理從4到n條所有的空間參考直線.然而,文獻[11]算法在空間直線較少的時候求解精度不高,這主要是因為其最優化求解過程中采用了線性化的策略.為了提高PnL 算法的求解精度,Mirzaei等[12]提出了一種直接求解PnL 問題全局最優解的方法,通過Cayley 參數的方式參數化旋轉矩陣,然后通過矩陣分解和合成的方式,將PnL 位姿測量問題轉換為最優化問題,最后通過矩陣合成技術求解這個最優化問題,得到PnL 問題的解.然而,文獻[12]的方法由于使用了Cayley 參數表示旋轉矩陣,求解過程中容易出現矩陣奇異值,導致求解穩定性不好.為了克服這個問題,Zhang等[13]提出了RPnL 方法求解PnL 問題,該方法將空間參考直線三條線為一組,然后利用Perspective-3-line (P3L)約束構建多項式方程組來求解PnL 問題.然而,RPnL 是一種次優化的方法,其求解精度還有提升的空間.2017 年,Xu等[9]改進了RPnL 方法,提出了ASPnL 算法.到目前為止,ASPnL 是求解精度最高,最穩定的PnL 算法之一.然而,ASPnL隨著直線數量的增加,其求解消耗時間將快速增長,不利于其應用在實時性較高的任務中.2019 年,Wang等[14]對RPnL 算法增加優化步驟,并且使用矩陣化的方式替代RPnL 算法中的循環步驟,提出了改進的SRPnL算法,該算法求解精度和可靠性類似于ASPnL 算法,但效率遠高于ASPnL 算法.

從以上文獻分析可以看出,線性求解PnL 問題的方法效率高,但精度低,通常無法適用于直線數量小于6 的情況.非線性求解PnL 的方法適應性較廣(適應4～n條直線求解),求解精度高,但是求解過程復雜,效率低下.針對以上問題,本文提出了一種同時兼具求解效率和求解精度的方法(命名為EPnL 方法).本文的主要貢獻有:

1) 提出了分類表示旋轉的方法: 最近的PnL問題求解中多使用Cayley 參數[13?15]和四元數表示[16]旋轉,然而利用Cayley 參數求解PnL 問題容易出現奇異值,導致求解結果不穩定.利用四元數表示旋轉則存在解的符號模糊問題,會增加PnL 問題求解的復雜性,降低求解效率.針對以上缺點,本文基于四元數參數中變量不同時為零的特性,提出了分類表示旋轉的方法,在保證不損失旋轉正交約束信息的前提下,避免了Cayley 參數求解奇異性和四元數參數對解符號模糊的問題,提升了求解PnL 問題的可靠性和效率.

2)將PnL 問題轉換為了二次曲面(曲線)方程組求交點的問題: 本文首先通過矩陣變化的方式統一求解參數的度量空間,消除求解參數度量空間不同可能引起的算法不穩定因素.然后,不同于現有文獻[12?14]直接最優化求解PnL 問題的方式,本文基于1)中分類表示旋轉的方法,將PnL 問題轉換為二次曲面(或曲線)求交點的問題來解決,有效地降低了PnL 問題求解的復雜度.

3)利用方程組低次項參數化高次項的方式將復雜二次曲面(曲線)方程組求交點問題轉換為單變量多項式求解問題: 針對迭代法求解耗時且容易陷入局部最優,而Gr?bner 基方法[17]求解復雜,無法保障可靠性的問題.本文利用二次曲面方程組自身的結構信息,將方程組劃分為高次項和低次項部分,并通過引入恒等式的方式將高次項用低次項表示,最終將復雜的多變量二次曲面求解問題轉換為簡單的單變量多項式(最高為8 次)求解問題來解決.同時,本文利用少量Gauss-Newton 法對結果進行精定位,以進一步提升最終的求解精度.

4)本文提出的算法實現了求解精度和效率的統一.實驗部分選擇和主流及最新的PnL 算法對比,結果表明,EPnL 適用于3～n條直線的位姿求解,具有通用性.在所有對比算法中,EPnL 算法求解精度最高,效率僅次于線性DLT 的方法(排名第2).本文中所有方法的源代碼已公布1https://sites.google.com/view/ping-wang-homepage,讀者可以下載驗證.

1 PnL 問題的描述

基于相機的透視成像模型,PnL 問題如圖1 表示,其中Li=(vi,Pi) 表示3D 空間中的某條已知直線,vi∈R3為Li的方向向量,Pi∈R3為Li上的任意一點.li=(si,pi) 為Li在圖像平面上的投影直線,si和pi分別為li的兩個端點.直線Li,li和相機光心O共同形成平面πi,且ni為平面πi的法向量.當相機內參數標定的情況下,ni很容易由光心和投影直線兩個端點所形成向量的外積得到,即:

圖1 PnL 問題Fig.1 PnL problem

由于直線Li在平面πi內,因此其和平面法向量ni滿足垂直的關系,即:

式中,R∈SO(3) 為3 × 3 矩陣,表示相機坐標系和空間直線所在坐標系(世界坐標系)之間的旋轉關系;t∈R3為3 × 1 向量,表示相機坐標系和世界坐標系之間的平移關系.式(2)可進一步展開為:

PnL 問題的目標就是在空中直線Li和其投影li已知的情況下,利用式(3)和式(4)計算R和t.

2 約束方程組的構建

PnL 問題中旋轉矩陣R和平移向量t的度量空間是不同的,這在優化求解過程中容易導致系數矩陣中元素數值的差異過大,最終影響PnL 問題的求解精度.由式(4)可以看出,平移t和旋轉R之間呈現線性關系.因此,本文采用矩陣變化的方式,基于空間所有的直線信息,利用旋轉R參數化平移t,進而統一PnL 問題中待求解參數變量的度量空間,以最終提升求解PnL 問題的可靠性和精度.式(3)進一步可以表示為:

綜合式(4)和式(5),將其寫為矩陣形式:

式(6)對空間中的每一條參考直線都滿足,因此有:

基于式(7),t可以表示為:

式中,B+=(BTB)?1BT為B的廣義逆.由式(8)中可以看出,B+,A和D中包含空間所有直線提供的已知信息,W由旋轉R構成.因此基于式(8),平移向量t可以表示為旋轉R的參數化形式.進一步將式(8)代入式(6)得到:

式(9) 依舊對空間的n條直線都滿足,將式(9)變為矩陣形式得到:

由式(10) 可以看出,A、D、B+、Ai、Di和Bi(i=1,2,···,n) 均可以由已知的ni、vi和Pi提前計算得到.式(10)中的未知數僅由C和W提供,而C和W由旋轉未知變量R構成.此時,如果能夠利用式(10)求解得到旋轉變量R,則平移向量t可以由式(8)給出,即PnL 問題得到求解.R和t的具體求解方法,本文將在第4 節重點展開研究.

3 基于四元數的旋轉分類表示法

由式(10)可以看出,R的表示形式直接影響著式(10)的復雜程度和求解系數矩陣的奇異性,進而間接影響優化求解PnL 算法的精度、可靠性和效率.R的表示形式通常包括: 歐拉角表示形式、旋轉矩陣表示形式、Cayley 參數表示形式、四元數表示形式、對偶四元數表示形式、角軸參數表示形式.其中四元數表示含有正交約束信息,且其形式不具有奇異性,因此利用四元數解決PnL 問題具有精度和可靠性高的特點.四元數表示R的形式如下:

式中,a、b、c和d為變量且滿足約束條件a2+b2+c2+d2=1.由式(11)可以看出,利用四元數求解旋轉時,[a,b,c,d]T和[?a,?b,?c,?d]T表示相同的旋轉,這說明四元數對變量的正、負號無法分辨,這無疑將擴大求解PnL 問題時解的搜索空間,增加求解問題時的復雜性,降低求解問題的效率.

由四元數約束a2+b2+c2+d2=1 可以看出,四元數在表示旋轉時,參數a、b、c和d是不同時為0 的.基于這個特點,根據a、b、c和d的取值,本文提出基于四元數的旋轉R分類表示方法:

1)形式1.當a、b、c和d都不為0,任選一個變量(如a),在R右端提取出 1 /a2且令s1=b/a,s2=c/d和s3=d/a,則R變為:

2)形式2.當a、b、c和d中有一個變量為0,例如a=0,b0,c0和0.則任選一個不為0 的變量,例如用b來化簡R,則可得:

式中,s2=c/b,s3=d/b,且H=

3)形式3.當a、b、c和d中有兩個變量為0,例如a=0 ,b=0,c0和d0 .任選c和d中的一個變量,如c化簡R得到:

式中,s3=d/c,并且有H=1+

4)形式4.當a、b、c和d中僅有一個變量不為0,例如a=0 ,b=0 ,c=0和0 ,則R直接表示為:

通過以上分類表示R的方式,在保證不損失四元數正交約束信息的前提下,避免了四元數對解符號模糊的問題,有效降低了求解PnL 問題時未知變量的個數和解空間的維度,在理論層面保障了求解PnL 問題的可靠性和效率.

4 旋轉矩陣和平移向量的求解

本節將旋轉R的4種形式分別代入式(10)進行求解.

1)對于形式1.將式(12)代入式(10),進一步展開,合并同類項得到:

式中,E1為已知的2n×10 矩陣,可以由A、D、B+、Ai、Di和Bi(i=1,2,···,n) 計算得到.β1=為式(12)的向量形式.對于式(16),可以采用Newton 法和Gauss-Newton法[6]等迭代法求解.然而,迭代法對初值的選擇敏感,初值選取不好的時候容易導致迭代陷入局部最優.此外,迭代法求解過程中需要耗費較多的時間.為克服此問題,最近的研究[15?16]中將類似式(16)的等式轉換為無約束最優化問題,通過Gr?bner 基方法[17]構建矩陣消去模板來求解.然而,Gr?bner 基方法求解式(16)時需要對其兩端取平方,這將引入較多高次未知數,導致Gr?bner 基方法構建的矩陣消去模板過于復雜,無法保證求解的可靠性.除此之外,矩陣消去模板越復雜,算法的計算效率也越低,并且復雜的矩陣消去模版更不利于算法的工程實現.

由式(16)可以看出,β1中含有3 個未知數,且所含未知數的最高次數為2.因此式(16)中每一行都代表著一個二次曲面,可以將求解式(16)看作為求解一組二次曲面交點的問題.觀察式(16)結構可以發現,將變量s1看作為常量,s2和s3為變量,則E1可以被劃分為兩部分:

式中,E1(i),i=1,2,···,10 表示E1的i列,G1(s)中的每一個元素都是變量s1的多項式.式(17)進一步可變為:

通過以上劃分方式,可以將式(16)中的高次項部分通過低次項部分來表示.利用式(16)中高次項和低次項之間的關系引入以下恒等式:

通過式(19)的恒等式,可以建立起式(16)中高次項之間的聯系.將式(18)再次代入式(19),展開可得:

將式(20)、式(21)和式(22)展開為:

式中,Ui,i=0,1,···,4 為系數.采用特征值分解的方法,式(34)很容易求解,一旦得到s3,將其代入式(14)和式(8)可得完整的R和t.

4)對于形式4.式(15)中R的值已知,將其直接代入式(8)可得t.

由以上求解可以看出,式(25) 有8 個解,式(31)有1 個解,式(34)有4 個解,求解形式4 有1 個解.因此,求解以上形式最多得到14 個解,其分別對應著14 組R和t. 將每組R和t分別代入式(10),選擇其誤差最小的那組R和t作為最終結果輸出.表1 列出了文獻中各非線性求解PnL 問題算法解的個數.從表1 可以看出,本文提出方法解的個數最少,有利于提升算法的求解效率和求解可靠性.

表1 解的個數對比Table 1 Comparison of the number of solutions

5 實驗結果

本節通過實驗驗證本文提出的EPnL 算法性能,并與現有主流算法進行對比.對比算法主要分為線性算法和非線性算法兩類,其中線性算法有DLT-Lines[7]和LPnL-Bar-LS[9]算法;非線性算法有 Lift[11]、AlgLS[12]、RPnL[13]、ASPnL[9]和SRPnL[14]算法.

實驗中的源代碼可以從作者個人網站https://sites.google.com/view/ping-wang-homepage 下載.

5.1 仿真實驗

合成分辨率為640 × 480 像素,焦距為800 像素的虛擬相機.在相機坐標系下產生空間3D 直線:在普通情況下,空間直線隨機分布在 [ ?2,2] ×[?2,2] × [ 4,8] 的范圍內;在共面情況下,空間3D直線隨機分布在 [ ?2,2] × [ ?2,2] × [ 0,0] 內.利用隨機產生的旋轉矩陣Rtrue和平移向量ttrue將相機坐標系下的3D 直線轉換到世界坐標系.利用合成的虛擬相機,將相機坐標系下的3D 直線投影到圖像平面上,并根據仿真實驗參數的不同,給投影直線增加不同等級的噪聲δ.為了誤差評估的一致性,本文使用文獻[9?15]的誤差定義形式:

式中,rk,true和rk分別表示Rtrue和R的第k列.

1)直線數量對算法精度的影響.首先設置仿真噪聲δ=5,通過改變輸入參考直線數量4～20 來驗證各PnL 算法的求解精度,結果如圖2 所示 (部分算法共面情況下幅度超出顯示范圍).由圖2 可以看出,線性的DLT-Lines和LPnL-Bar-LS 方法在普通和共面情況下求解精度都不高,且在直線數量少于6 時無法正常求解.Lift 是一種非迭代的方法,但由于其求解過程中使用了線性化的策略,因此在普通情況(共面情況下無法求解)下求解精度不高.AlgLS 的中值求解精度很高,但是其平均求解精度反而較低,原因是AlgLS 方法采用了Cayley 參數表示旋轉矩陣,導致計算的時候容易出現奇異性,影響算法的整體計算精度.RPnL 是一種次優化方法,在普通情況下求解精度較高,在共面情況下求解精度較差.ASPnL 算法作為RPnL 算法的改進版本,在普通情況下能取得很高的求解精度,但在共面情況下平均求解精度較低.SRPnL 方法作為RPnL 方法的另一種改進版本,在普通和共面情況下都能取得較高的求解精度.相比之下,本文的EPnL 采用了全局優化的方式求解,并且在求解過程中分類考慮了旋轉的情況,避免了求解過程中的奇異性,因此在普通和共面情況下都能保證最好的求解精度.

2)噪聲對算法精度的影響.固定空間直線數量為10,改變噪聲等級δ為1～15 來驗證各PnL 算法的求解精度,結果如圖3 所示.可以看出,隨著噪聲等級的增加,各算法的求解精度都在下降,且噪聲等級和求解誤差的增加近似符合線性關系.由圖2～3 可以看出,本文PnL 算法在普通情況和共面情況下都能取得最好的求解精度.

圖2 當直線數目變化時旋轉和平移誤差的均值和中值Fig.2 The mean and median of rotation and translation errors when the number of lines varies

圖3 當噪聲等級變化時旋轉和平移誤差的均值和中值Fig.3 The mean and median of rotation and translation errors when the noise level varies

3)對比P3L 算法.本文算法雖然不是為求解P3L 問題設計的,但是可以處理P3L 問題.圖4 為本文算法和現有最新P3L 算法[13,19]的對比結果.由圖4 可以看出,本文EPnL 算法不僅能夠處理P3L問題,而且還具有較高的求解精度.

圖4 最小情況下(n =3)旋轉和平移誤差的均值和中值Fig.4 The mean and median of rotation and translation errors in the minimal case (n =3)

4)對比運行效率.圖5 為各PnL 算法的運行效率曲線圖.測試直線的數量從4 到2 000 條,足夠覆蓋大多數的實際應用場合.測試時每種算法分別執行500 次,并統計其平均運行時間.由圖5 可以看出,RPnL和ASPnL 算法初期運行效率高于AlgLS 的方法,但是隨著直線數目增加,其消耗的時間呈指數狀增加,效率反倒低于AlgLS 方法.DLT-Lines和LPnL-Bar-LS 由于是線性求解的方法,因此求解效率很高.SRPnL 方法相比于RPnL和ASPnL 方法具有較高的求解效率,但是隨著直線數量的增加,其時間消耗也急劇增加.相比之下,本文提出的EPnL 方法具有很高的求解效率,當直線數量超過300 條的時候,求解效率甚至高于線性的DLT-Lines 的方法,僅次于LPnL-Bar-LS 方法.綜合考慮EPnL 算法的求解精度、求解效率和求解通用性,其在所有對比方法中綜合性能更優,非常適合于實際項目應用.

圖5 對比算法的計算效率Fig.5 The computational efficiency of compared the methods

5.2 實際實驗

牛津大學視覺測量組(Visual Geometry Group)建立了以其團隊命名的(VGG)圖像數據集2http://www.robots.ox.ac.uk/～vgg/.VGG數據集中包含7 組圖像數據,每組數據中包含若干張采集的建筑物圖像,圖像中建筑物邊緣直線的圖像坐標及其對應世界坐標系中的位置、相機相對于世界坐標系之間的位姿關系都是已知的.因此可以使用VGG 數據集來測試各PnL 算法的求解精度.測試過程中各算法的求解誤差定義如下:

式中,Angle(·) 表示將旋轉矩陣轉換為歐拉角,Restimation和testimation表示各算法計算得到的旋轉矩陣和平移向量,Rtrue和ttrue為數據集中真實的旋轉矩陣和平移向量.分別利用各PnL 算法按式(36)計算每組圖像數據的誤差平均值,結果如表2 所示,每組中平均誤差最小的值加粗表示.由表2 可以看出,本文提出的EPnL 算法在能夠在4組測試集上同時獲得最高的角度(旋轉)和位置(平移)求解精度,并且能夠在6 組測試集上獲得最高的位置求解精度,其整體求解精度最好.為進一步驗證EPnL 算法的求解精度,利用EPnL 算法求解的位姿信息將世界坐標系下建筑物的邊緣投影到圖像上,結果如圖6 所示,其中青色直線為投影直線.由圖6 可見,EPnL 算法能夠準確的恢復位姿信息.

圖6 VGG 數據集中的圖片Fig.6 Images from the VGG dataset

表2 各算法在VGG 數據集上的旋轉和平移誤差均值Table 2 The mean of rotation and translation errors for each method on the VGG dataset

6 結束語

本文提出了一種精確且高效的PnL 問題求解算法(EPnL 算法).EPnL 首先通過矩陣變化的方式統一PnL 問題的度量空間,并將PnL 求解問題轉換為求二次曲面(曲線)交點的問題.然后,針對現有Cayley 參數和四元數參數表示旋轉時存在的問題,本文提出了基于四元數的旋轉分類表示方法,該表示法在不損失旋轉正交約束的前提下,能夠有效提升求解PnL 問題的可靠性和效率.最后,針對現有迭代法和Gr?bner 基法求解問題效率不高且無法保障可靠性的問題.本文提出利用二次項曲面方程組低次項參數化高次項的方法,將二次項曲面方程組求交點問題轉換為單變量多項式求解問題解決.仿真和實際實驗表明,相比于現有的PnL算法,本文算法在具有高求解精度的同時兼具高求解效率.