基于四元數(shù)最少特征點的相機位姿估計

侯培國， 趙梓建， 宋 濤,2

(1.燕山大學(xué) 電氣工程學(xué)院，河北 秦皇島 066004； 2.秦皇島視聽機械研究所，河北 秦皇島 066004)

1 引 言

計算機視覺是利用相機以及計算機來模擬人眼功能，使得計算機智能化，能夠從平面圖像以及三維立體圖像中提取出操作者所需要的有用信息，并進一步做分析處理[1～3]。計算機視覺的應(yīng)用越來越廣泛，而準確的位姿估計在計算機視覺領(lǐng)域中起著十分重要的作用。例如，在計算機視覺的測量中，通過了解相機的位姿變化，可以從一組二維的圖像中構(gòu)建出相應(yīng)的三維場景模型[4]；在機器人技術(shù)中，相機位姿可以用于機器人的導(dǎo)航系統(tǒng)[5]，使得機器人具有“方向感”；除此之外，相機位姿估計在同步定位、自動駕駛汽車以及增強現(xiàn)實等場合中也具有廣泛應(yīng)用[6～8]。

相機位姿估計需要能夠在兩個或多個圖像中檢測到圖像的特征點并匹配成功，例如圖像的邊緣、角點等[9]，并且要在空間的一般位置找到至少5個匹配的特征點。當(dāng)檢測并匹配到6個以及更多個特征點進行位姿估計時，位姿估計問題是線性的。而基于5個特征點的相機位姿估計算法(下文簡稱為5點算法)則必須要解決多項方程組非線性問題[10]。因此，在求解位姿時前一種算法往往比5點算法更快。雖然基于6個或更多個特征點進行相對位姿的估計的執(zhí)行時間比較短，但是這種算法經(jīng)常會“失效”，導(dǎo)致算法在許多場景中無法正確求解位姿。例如，利用8點算法進行相機位姿求解時，如果匹配的特征點是共面，或者當(dāng)相機僅具有旋轉(zhuǎn)動作而平移值為零時，8點算法就不能準確求解。但是5點算法在這樣的情況下仍可以很好地求解相對位姿。

許多現(xiàn)有方法使用特征點的坐標來構(gòu)造基本矩陣，從中可以求解相機的旋轉(zhuǎn)和平移[11，12]。Nister將5點姿態(tài)估計問題表述為4個未知變量中的多項式方程組，提出了一種算法，將該系統(tǒng)表示為一個變量的10次多項式，從多項式的根來獲得系統(tǒng)的解[13]；Li和Hartley對Nister算法進行了改進，使得改進的算法具有更穩(wěn)定的數(shù)值和更快的執(zhí)行時間[14]；Kukelova等使用求解多項式特征值的方法進行位姿估計[15]。除此之外，8點算法也是實用的相機運動估計算法之一[16]，算法在基本矩陣中體現(xiàn)的極線約束下，由8個或更多個特征點生成的線性方程組進行位姿估計。

以上方法在進行求解時都是基于基本矩陣來進行位姿估計。但是，當(dāng)兩個相機視圖之間的平移值變?yōu)榱慊虍?dāng)匹配的特征點共面時，基于基本矩陣進行位姿估計的方法往往會失去精度[17]。近年來，人們已經(jīng)提出了獨立于平移矩陣來估計相機旋轉(zhuǎn)的新算法,算法不依賴于基本矩陣，從而能避免了相機的旋轉(zhuǎn)和平移糾纏在一起的問題。Kalantari等提出一種獨立計算相機旋轉(zhuǎn)和平移的算法[18]，但是該方法效率很低，應(yīng)用效果不太理想;Kneip等在其基礎(chǔ)上提出了一種改進的更有效的算法[19]，但是該算法在相機相對平移較小時很容易受到影響，導(dǎo)致位姿估計的誤差較大。

本文提出了一種利用四元數(shù)直接估計相機位姿的算法。算法檢測相機圖像中的特征點，并進行特征點自動匹配，獲得特征點的二維坐標。利用四元數(shù)表示旋轉(zhuǎn)矩陣R，建立并求解多項式系統(tǒng)。獲得四元數(shù)的變量值后，進一步求解得到旋轉(zhuǎn)矩陣t和深度信息。對算法在噪聲影響下進行測試，并且在多特征點的情況下對算法性能進行了評估，通過與其他常用算法的精度比較顯示出文章算法的優(yōu)越性。最后通過KITTI數(shù)據(jù)集來測試算法的實際估計精度。

2 相機位姿估計

利用特征點進行位姿估計首先要從相機圖像中檢測并匹配特征點。如圖1所示。相機在不同位置所拍攝的兩幅圖片，利用計算機在兩幅圖像中進行特征點的檢測和匹配，同時獲取特征點的坐標信息。左側(cè)圖中檢測到的特征點是紅色，右側(cè)為綠色。將特征點進行匹配并用黃色線段進行連接，以圖像中心為圓心，向右為x軸正方向，向下為y軸正方向，則特征點坐標可知。

圖1 特征點匹配圖Fig.1 Matched feature points

2.1 多項式方程組的建立

設(shè)R∈SO(3)和t∈R3分別表示視圖之間相機的相對旋轉(zhuǎn)矩陣和平移矩陣。根據(jù)相機在兩個視圖拍攝的場景圖像，可以檢測并匹配圖像中的特征點，并從圖像中提取它們所在圖像平面中的坐標。坐標首先以像素為單位獲得，然后通過相機校準矩陣將坐標映射到圖像平面上的笛卡爾坐標。對于每個匹配的特征點，都遵守剛性運動約束：

uRp+t=vp′

(1)

式中：p，p′∈R3代表兩個圖像中相匹配的特征點的齊次坐標。標量u和v代表空間中的三維特征點沿著相機坐標系的z軸到原點的距離，稱為每個視圖處的三維特征點的深度值，如圖2所示。點p和p′坐標可以從圖像中得到，為已知參數(shù)。因此式(1)中需要求解的未知數(shù)有u，v，R和t。

圖2 不同視角下的特征點Fig.2 Characteristic points from different perspectives

(2)

用四元數(shù)的變量表示旋轉(zhuǎn)矩陣，就可以對其直接求解。這里將四元數(shù)中的第一個變量定位為非負數(shù)，即w≥0，則旋轉(zhuǎn)矩陣和四元數(shù)之間存在一對一的對應(yīng)關(guān)系。要進行位姿求解，首先消除未知的參數(shù)：u，v和t，并根據(jù)四元數(shù)的變量導(dǎo)出方程組。通過求解該系統(tǒng)，就可以求解得到旋轉(zhuǎn)矩陣，將所有特征點都代入式(1)可以得到關(guān)于平移和深度值的矩陣表達式，對矩陣方程進行求解就可以得到特征點的平移和深度信息。

2.1.1 消除未知參數(shù)

對旋轉(zhuǎn)矩陣和平移矩陣求解，首先需要消除方程式中的一些參數(shù)。將圖像中2個不同的特征點代入式(1)可以得到2個方程式，通過2個方程式相減就可以消除未知參數(shù)t。類似的，將3個特征點代入式(1)就可以得到3個方程式，通過公式之間相減就可以消去各個公式中的未知參數(shù)t。將圖1所示的特征點F1(-0.8，0.3)，F(xiàn)2(1.0，1.4)，F(xiàn)3(2.5，0.9)以及各自相匹配的F′1(-1.3，0.4)，F(xiàn)′2(0.3，1.5)，F(xiàn)′3(1.9，0.8)代入式(1)，可得：

(3)

(4)

(5)

式(3)分別與式(4)和式(5)相減可以得到不含未知數(shù)t的方程：

(6)

(7)

將未知數(shù)u，v看作未知數(shù)，就可以將方程表示為MV=0：

(8)

得到的矩陣M僅由特征點坐標和旋轉(zhuǎn)矩陣R組成，向量V由特征點的深度參數(shù)組成。矩陣M的行列式值等于零，對其進行行列式求值，就可以列出關(guān)于四元數(shù)變量w，x1，x2，x3的4次多項式方程，其系數(shù)與特征點坐標有關(guān)。多項式方程中不含有未知數(shù)u，v，這樣就消除了未知的深度參數(shù)，如式(9)所示：

(9)

2.1.2 建立方程組

4次多項式方程(9)由35個單項式構(gòu)成，定義運算：

(10)

則一個擁有c個變量的d次多項式由〈d+c-1，c-1〉個單項式構(gòu)成。由于任意3個特征點就可以得到一個多項式方程，那么k個特征點可以得到〈k，3〉個方程式。用四元數(shù)的變量w，x1，x2，x3分別乘以所得的4次多項式方程，則可以得到更多的多項式方程，如式(11)所示。

(11)

若利用5個特征點來進行相機的位姿估計，則可以確立〈5，3〉=10個4次多項式方程。將四元數(shù)變量分別與其相乘，則可以得到40個5次方程式，其單項式的數(shù)量為〈4+5-1，3〉=56。

2.2 旋轉(zhuǎn)矩陣的求解

將得到的新方程組用矩陣向量形式表示為：AX=0，其中矩陣A由方程組系數(shù)組成，A∈R40×56。矩陣X由所有5次單項式組成，X∈R56。如下所示：

(12)

將向量X分成2個向量：

其中X1∈R35，X2∈R21。X1是包含w的所有單項式的向量，X2則由其余的單項式組成。令A(yù)=[A1A2]，其中A1由與X2相關(guān)聯(lián)的A的列組成，A2∈R40×35；并且A2由與X2相關(guān)聯(lián)的A的列構(gòu)成，A2∈R40×21。則系統(tǒng)AX=0可以等效地寫為：

A1X1+A2X2=0

(13)

(14)

將向量X1中的變量w提出，并用V表示，即:

(15)

構(gòu)造方陣B，B∈R35×35。構(gòu)造矩陣方程：λV=BV，將問題轉(zhuǎn)化為特征值問題。

(16)

2.3 平移和深度信息的求解

求得相應(yīng)的旋轉(zhuǎn)矩陣R后，就可以進一步求解特征點的平移和深度值。所有匹配的特征點都遵守剛性運動約束(1)，則對于k個特征點，將其表達為成矩陣向量形式CY=0：

(17)

式中：I∈R3×3是單位矩陣；k是特征點的數(shù)量；矩陣C∈R3k×2k+3，Y∈R2k+3。從式(17)中可以知道矩陣Y在矩陣C的零空間中。通過計算矩陣C的右奇異向量，即CΤC對應(yīng)的特征向量就可以求解得到Y(jié)。向量Y中同時包含了平移信息和深度信息，因此，這些參數(shù)可以同時求解得到，并且約束到同一個比例因子。

2.4 算法分析

3 算法性能測試

將算法和幾種現(xiàn)有算法進行性能測試。測試利用蒙特卡羅模擬，在相機前方矩形平行六面體內(nèi)，分別隨機生成分布均勻的一般三維點和共面三維點的混合集合。然后通過隨機的平移和旋轉(zhuǎn)將相機移動到其他的位置。利用針孔照相機模型，通過將三維空間點投影到圖像平面來計算特征點的坐標。文中使用張正友標定法對相機進行標定，并且相機標定所得矩陣為K。每個圖像是1 728×668像素。

將文中算法與8點算法、Kukelova算法、Nister算法、Li和Hartley算法以及Stewenius算法[22]進行性能比較。在試驗測試中比較各個算法的旋轉(zhuǎn)誤差與平移誤差來展示比較結(jié)果，并將結(jié)果繪制成曲線的形式。具體誤差計算方法如下：

定義公式：

(18)

式(18)用于描述旋轉(zhuǎn)誤差，ρ(q，q*)∈[0，1]。其中q=[wx1x2x3]Τ是測試中用來求解旋轉(zhuǎn)矩陣的四元數(shù)值，而q*是真實值。類似地，通過將式中的四元數(shù)向量替換為單位范數(shù)平移矩陣即可求解得到平移誤差。

3.1 抗噪聲測試

為評估算法在噪聲影響下的性能，實驗使用均值為零、標準偏差為0～3個像素的高斯噪聲模擬噪聲干擾。將其加到所有圖像中并設(shè)置噪聲標準偏差增量值為0.1個象素值，對每個噪聲增量值進行100次隨機實驗。選擇最接近真實值的解作為每個算法的估計值。為比較這幾種算法在最少特征點時對于噪聲的抗干擾能力，實驗時為參與比較的算法提供的特征點數(shù)目為各個算法求解時所需的最少特征點數(shù)量。使用式(18)可得到各算法對應(yīng)的旋轉(zhuǎn)誤差和平移誤差，對應(yīng)的誤差曲線如圖3所示。

從圖3可以看到Nister算法、Li和Hartley算法以及Kukelova算法估計的誤差比較接近，因此它們的誤差曲線幾乎重疊。8點算法展現(xiàn)出了較為良好的平移誤差，但是由于其旋轉(zhuǎn)誤差太大，圖3中只顯示了其旋轉(zhuǎn)誤差曲線的一部分。Stewenius算法表現(xiàn)出良好的估計精度，在比較中排名第二。本文算法的誤差曲線位于所有曲線下方，估計誤差要小于其他算法，顯示了最佳性能。

圖3 抗噪聲比較結(jié)果Fig.3 Result of anti-noise comparison

3.2 多特征點性能測試

在實際應(yīng)用中，在兩幅圖像之間檢測和匹配的特征點數(shù)量通常大于算法所需的最小數(shù)量。因此，在實際應(yīng)用中，位姿估計算法往往要解決特征點數(shù)超過最小值的問題，以減少噪聲和錯誤匹配的影響。實驗測試時提供特征點數(shù)目為從各自算法所需的最少特征點數(shù)到100個特征點來模擬實際中算法匹配到多個特征點的情況，并設(shè)置特征點數(shù)增量為1。每次增加一個特征點都執(zhí)行100次比較試驗，并且將標準偏差為0.75像素的高斯噪聲添加到像素坐標，以模擬圖像像素化噪聲和特征點檢測和匹配中的不準確性。使用式(18)求出對應(yīng)的旋轉(zhuǎn)誤差和平移誤差，得到的誤差曲線如圖4所示。

圖4 多特征點性能比較結(jié)果Fig.4 Performance comparison results of many feature points

因為Li和Hartley的算法不接受超過5個特征點，因此沒有參與比較。如圖4所示，在旋轉(zhuǎn)誤差曲線圖中：當(dāng)使用特征點數(shù)不超過10時，本文算法和Stewenius算法位姿估計精度最好，隨著特征點數(shù)目增加，8點算法的性能高于Stewenius算法，而本文算法的誤差曲線始終位于最下方，具有最好的估計精度；在平移誤差曲線圖中，當(dāng)特征點數(shù)超過10時，本文算法的估計精度則明顯的優(yōu)于其他參與比較的算法?？傮w來看，本文算法的旋轉(zhuǎn)和平移估計誤差曲線都在其他算法下方，估計精度最高，顯示出最佳性能。并且隨著匹配到的特征點數(shù)量增加，估計結(jié)果誤差會進一步降低。

3.3 真實數(shù)據(jù)集測試

為測試算法的實際應(yīng)用效果，使用來自真實世界數(shù)據(jù)集的圖像測試6種算法，并將估計結(jié)果與數(shù)據(jù)集提供的真實值進行比較。測試采用KITTI數(shù)據(jù)集[22]。KITTI數(shù)據(jù)集是最大的計算機視覺算法評測數(shù)據(jù)集之一，由卡爾斯魯厄理工學(xué)院和豐田美國技術(shù)研究院聯(lián)合創(chuàng)辦，包含了城市、鄉(xiāng)間、公路等等眾多不同的場景下采集的真實圖像數(shù)據(jù)[23]。

使用數(shù)據(jù)集測試時，在不影響測試結(jié)果的前提下，采用抽樣原則，對數(shù)據(jù)集設(shè)置增量為5。這樣可以縮減測試時間，同時增大視差，更加利于平移信息的估計。所有算法實現(xiàn)都在Matlab中使用C語言進行編程實現(xiàn)，并執(zhí)行為MEX文件。測試在同一計算機平臺上進行。計算機采用Intel(R)Core(TM)i5-8500CPU，主頻為3.00 GHz，內(nèi)存容量為8 GB。結(jié)果如表1和表2所示。

表1 平均旋轉(zhuǎn)誤差Tab.1 Average rotation error

表2 平均平移誤差Tab.2 Average rotation error

為了反映測試結(jié)果的統(tǒng)計信息，在表格中列出了平均旋轉(zhuǎn)誤差和平均平移誤差的四分之一處值，中值以及四分之三處的值，分別用A、B、C表示。同時為了便于比較，表1中的平均旋轉(zhuǎn)誤差為放大100倍后的結(jié)果。表中數(shù)據(jù)顯示，在參與比較的幾種算法中，本文算法的平均旋轉(zhuǎn)誤差和平均平移誤差均是最小的。與8點算法相比，本文算法的平均旋轉(zhuǎn)中值誤差降低了52.9%，平均平移中值誤差降低了13.4%。與排名第二的Stewenius算法相比，本文算法的平均旋轉(zhuǎn)中值誤差降低了24.5%，平均平移中值誤差降低了30.1%。通常來說，使用最少特征點數(shù)目大于5的算法往往比5點法算具有更短的執(zhí)行時間。本文算法執(zhí)行時間并非最優(yōu)，但這只是相對而言的。通過真實世界數(shù)據(jù)集的圖像的測試，本文算法完全可以滿足應(yīng)用中的實時性。

4 總 結(jié)

文中提出一種基于四元數(shù)最少特征點的相機位姿估計算法。對相機圖像進行檢測并匹配特征點，通過特征點的剛性約束條件建立方程組，從而構(gòu)建特征值問題來求解相機位姿。算法將特征點平移信息與深度信息約束到同一比例，因此當(dāng)攝像機視圖之間的距離消失時，也可以很好地進行位姿估計。本文算法通過矩陣的偽逆操作來抑制干擾，有效提高位姿估計的精度。利用軟件仿真隨機生成三維特征點，將本文算法與幾種現(xiàn)有的相機位姿估計算法的性能進行了全面比較。在比較中，本文算法的位姿估計精度最優(yōu)，顯著提高了相機旋轉(zhuǎn)與平移估計的精度，并且利用KITTI數(shù)據(jù)集測試算法的實用性能。將算法估計結(jié)果與其所提供的真實值作比較來驗證算法精度,從結(jié)果可以看出本文算法具有很高的估計精度，能夠很好地滿足實際應(yīng)用。