基于擴(kuò)展正交迭代的快速加權(quán)的相機(jī)位姿估計(jì)

王世安，王向軍，陰 雷

〈系統(tǒng)與設(shè)計(jì)〉

基于擴(kuò)展正交迭代的快速加權(quán)的相機(jī)位姿估計(jì)

王世安，王向軍，陰 雷

（天津大學(xué) 精密測(cè)試技術(shù)及儀器國(guó)家重點(diǎn)實(shí)驗(yàn)室，天津 300072）

相機(jī)位姿估計(jì)算法多基于參考點(diǎn)而較少利用圖像中的直線信息，本文對(duì)于相機(jī)位姿估計(jì)算法的抗干擾性和實(shí)時(shí)性，在擴(kuò)展正交迭代的基礎(chǔ)上，提出了一種基于點(diǎn)和直線段結(jié)合的快速加權(quán)的相機(jī)位姿估計(jì)算法，該算法以加權(quán)共線性誤差和加權(quán)共面性誤差之和為誤差函數(shù)，根據(jù)計(jì)算初值的深度信息和重投影誤差確定權(quán)重系數(shù)，并對(duì)整體進(jìn)行加速優(yōu)化，將每次迭代計(jì)算的時(shí)間復(fù)雜度從()降到了(1)。仿真實(shí)驗(yàn)結(jié)果表明算法可以抑制異常點(diǎn)的干擾，減少計(jì)算時(shí)間，旋轉(zhuǎn)矩陣計(jì)算誤差比傳統(tǒng)正交迭代算法減少48.31%，平移向量計(jì)算誤差減少48.79%，加速優(yōu)化后的計(jì)算時(shí)間為加速前的47.11%。實(shí)物實(shí)驗(yàn)表明該算法可以充分利用檢測(cè)到的參考點(diǎn)和參考直線信息，提高計(jì)算精度，有較高的實(shí)際應(yīng)用價(jià)值。

機(jī)器視覺；圖像處理；位姿估計(jì)；迭代算法；計(jì)算復(fù)雜度

0 引言

相機(jī)位姿估計(jì)問題是攝影測(cè)量學(xué)[1]、計(jì)算機(jī)視覺[2]、計(jì)算機(jī)圖形學(xué)[3]以及機(jī)器人科學(xué)[4]的一個(gè)重要問題，在無人平臺(tái)導(dǎo)航[5]、航天器對(duì)接[6]、相機(jī)標(biāo)定[7]、增強(qiáng)現(xiàn)實(shí)[8]等領(lǐng)域有廣泛應(yīng)用。位姿估計(jì)問題根據(jù)相對(duì)應(yīng)特征信息的不同可以分為3類[9]：2D-2D位姿估計(jì)、2D-3D位姿估計(jì)、3D-3D位姿估計(jì)。其中2D-3D位姿估計(jì)問題又叫做點(diǎn)透視問題（PnP，Perspective-n- Point）。

目前在PnP問題的迭代算法中，文獻(xiàn)[10]提出的正交迭代算法計(jì)算精度高，迭代次數(shù)少，計(jì)算速度快并且有全局收斂性。直線作為特征點(diǎn)之外易于獲取且較為穩(wěn)定的幾何特征在位姿估計(jì)中可以提高算法精度和魯棒性，并且在缺少紋理、圖案重復(fù)的情況下可以起到主導(dǎo)作用[11]。此外在復(fù)雜真實(shí)環(huán)境中進(jìn)行實(shí)時(shí)位姿估計(jì)時(shí)，由于測(cè)量誤差難免會(huì)產(chǎn)生異常數(shù)據(jù)，正交迭代算法的計(jì)算結(jié)果會(huì)有較大偏差。本文對(duì)于文獻(xiàn)[12]提出的基于點(diǎn)和直線段對(duì)應(yīng)的擴(kuò)展正交迭代算法進(jìn)行了優(yōu)化，通過初值得到普通特征點(diǎn)和直線段上特征點(diǎn)的深度和重投影誤差，通過初值的深度信息和重投影誤差確定物方空間誤差的權(quán)重系數(shù)，并根據(jù)文獻(xiàn)[13]提出的思想，提出了在點(diǎn)和直線段對(duì)應(yīng)情況下的快速加權(quán)正交迭代算法，將算法迭代時(shí)的計(jì)算復(fù)雜度從()降低到(1)，在提高計(jì)算速度的同時(shí)，提高了算法的抗干擾能力和旋轉(zhuǎn)矩陣、平移向量的計(jì)算精度，并可以克服場(chǎng)景中缺少特征點(diǎn)的情況。

1 基于點(diǎn)和直線段結(jié)合的快速加權(quán)正交迭代算法

1.1 擴(kuò)展正交迭代算法

正交迭代算法[10]計(jì)算參考點(diǎn)的共線性誤差，并以此作為迭代計(jì)算的誤差函數(shù)，即：

令v為參考點(diǎn)在歸一化平面上的投影，則公式中為單位矩陣；＝T/(T)為視線投影矩陣；為旋轉(zhuǎn)矩陣；為平移向量，且旋轉(zhuǎn)矩陣為正交矩陣，有T＝。

圖1 物體空間和歸一化平面上的對(duì)應(yīng)直線段

圖2 物體空間的共面性誤差

則將直線段L在物體空間的誤差的平方定義為其兩個(gè)端點(diǎn)的共面性誤差的平方之和：

設(shè)位姿估計(jì)前可以提取到個(gè)參考點(diǎn)(1,2, …,p)和個(gè)參考直線段(1,2, …,L)，則擴(kuò)展正交迭代算法的目標(biāo)函數(shù)為：

式中：＝＋2*，T＝，INT表示取整數(shù)。

1.2 加權(quán)擴(kuò)展正交迭代

1.2.1 加權(quán)擴(kuò)展正交迭代算法

擴(kuò)展正交迭代對(duì)于每一個(gè)參考點(diǎn)和每一條檢測(cè)到的直線段都賦予相同的權(quán)重，然而對(duì)于在相機(jī)坐標(biāo)系中不同深度的參考點(diǎn)和直線段，這樣顯然無法得到最高的精度。此外在很多情況下個(gè)別的異常參考點(diǎn)（包括參考直線段的端點(diǎn)）會(huì)有較大的測(cè)量誤差，在實(shí)際應(yīng)用中這種難以避免的誤差會(huì)較大地影響位姿估計(jì)算法的計(jì)算精度。因此可以對(duì)不同的參考點(diǎn)和直線段加入不同的權(quán)重系數(shù)，以提高計(jì)算精度避免異常點(diǎn)的影響。加入權(quán)重系數(shù)以后的目標(biāo)函數(shù)如下：

式中：＞0為第個(gè)參考點(diǎn)（包括直線段的兩個(gè)端點(diǎn)）的權(quán)重系數(shù)。加入權(quán)重后，當(dāng)給定第次對(duì)于的估計(jì)值(k)以后，可以得出此時(shí)的最優(yōu)值為：

1.2.2 權(quán)重系數(shù)確定方法

式中：為相機(jī)內(nèi)參矩陣。

式中：為r2的均值。由此可以得到既考慮了參考點(diǎn)深度又考慮了初始值的重投影誤差的權(quán)值：

1.3 快速加權(quán)擴(kuò)展正交迭代

正交迭代算法作為非線性PnP算法計(jì)算時(shí)間較長(zhǎng)，在對(duì)每個(gè)參考點(diǎn)加入權(quán)值以及擴(kuò)展到直線段以后又增加了較大的計(jì)算量，計(jì)算消耗時(shí)間進(jìn)一步增加，本文通過將迭代算法公式中的矩陣?yán)?，并通過整合使加權(quán)擴(kuò)展正交迭代算法每次迭代的計(jì)算復(fù)雜度從()降到了(1)，具體推導(dǎo)過程如下：

首先引入公式：

式中：vec表示以列為單位把一個(gè)矩陣排為一個(gè)列向量，稱為矩陣的拉直：?表示克羅內(nèi)克積（Kronecker product）。對(duì)式(9)做拉直運(yùn)算得：

式中：

進(jìn)一步可得：

式中：

整理公式得：

對(duì)進(jìn)行拉直運(yùn)算可以得到：

式中：

因此可以得到，對(duì)進(jìn)行奇異值分解（SVD）即可得到(k＋1)。

在每次迭代過程中對(duì)誤差函數(shù)進(jìn)行規(guī)整可得：

式中：

經(jīng)過對(duì)、和(,)進(jìn)行拉直運(yùn)算后，將每次迭代的變量提取出來，可以在迭代前只計(jì)算一次、和C，計(jì)算復(fù)雜度均為O()，在迭代時(shí)只需與拉直后的相乘，計(jì)算復(fù)雜度為O(1)，從而可以極大地減少加權(quán)擴(kuò)展正交迭代算法的計(jì)算時(shí)間，增強(qiáng)算法的實(shí)時(shí)性。

2 實(shí)驗(yàn)

為了驗(yàn)證本文提出的快速加權(quán)的擴(kuò)展正交迭代算法的正確性和有效性，我們進(jìn)行了相機(jī)位姿估計(jì)的matlab數(shù)值仿真實(shí)驗(yàn)和實(shí)物實(shí)驗(yàn)，分別比較以下9種算法的計(jì)算誤差和消耗時(shí)間：（1）RPnP：[15]提出的RPnP算法；（2）OPnP：[14]提出的OPnP算法；（3）LHM：[10]提出的以弱透視模型（Weak Perspective Approximation）為初值的正交迭代算法；（4）RPnP/OPnP＋LHM：使用RPnP和OPnP算法按照1.2.2小節(jié)的方式計(jì)算迭代算法的初值，然后使用正交迭代算法；（5）RPnP/OPnP＋ELHM：使用上述初值，然后使用[12]提出的擴(kuò)展正交迭代算法；（6）RPnP/OPnP＋D1＋ELHM：使用上述初值，然后使用只考慮深度信息的加權(quán)擴(kuò)展正交迭代；（7）RPnP/OPnP＋D2＋ELHM：使用上述初值，然后使用只考慮投影誤差的加權(quán)擴(kuò)展正交迭代；（8）RPnP/OPnP＋W＋ELHM：使用上述初值，然后使用考慮兩種權(quán)重系數(shù)的加權(quán)擴(kuò)展正交迭代；（9）RPnP/OPnP＋W＋ELHM＋：使用上述初值，然后使用經(jīng)過加速運(yùn)算的快速加權(quán)擴(kuò)展正交迭代。

2.1 仿真實(shí)驗(yàn)

仿真實(shí)驗(yàn)使用的虛擬相機(jī)的參數(shù)：分辨率為640×480，等效焦距為800pixel，參考點(diǎn)和參考直線段的端點(diǎn)隨機(jī)分布在[－2,2]×[－2,2]×[4,8]的空間中。仿真實(shí)驗(yàn)中每次隨機(jī)產(chǎn)生3個(gè)歐拉角并將其轉(zhuǎn)換成旋轉(zhuǎn)矩陣，在[－50,50]×[－50,50]×[0,100]的范圍內(nèi)隨機(jī)產(chǎn)生平移向量，并將點(diǎn)投影在圖像上后加入期望為0，標(biāo)準(zhǔn)差為的高斯噪聲。對(duì)于旋轉(zhuǎn)矩陣，由于歐拉角的萬向鎖問題（Gimbal Lock），實(shí)驗(yàn)中將旋轉(zhuǎn)矩陣轉(zhuǎn)換成四元數(shù)來表示姿態(tài)，令旋轉(zhuǎn)矩陣真值轉(zhuǎn)換成為四元數(shù)，計(jì)算得到的旋轉(zhuǎn)矩陣轉(zhuǎn)換￠成為四元數(shù)￠，定義旋轉(zhuǎn)矩陣的相對(duì)誤差為()＝(－￠)2/2。對(duì)于平移向量，令平移向量真值為，計(jì)算得到的平移向量為￠，定義平移向量的相對(duì)誤差為()＝(－￠)2/2。

2.1.1 參考點(diǎn)數(shù)目實(shí)驗(yàn)

參考點(diǎn)數(shù)目實(shí)驗(yàn)（研究存在異常參考點(diǎn)時(shí)算法計(jì)算精度和參考點(diǎn)數(shù)目的關(guān)系）：參考點(diǎn)數(shù)目從4增加到12，參考直線數(shù)目為2，隨機(jī)一個(gè)參考點(diǎn)加上為5pixel的高斯噪聲，其余參考點(diǎn)加上為0.5pixel的高斯噪聲。對(duì)于不同的參考點(diǎn)數(shù)目都進(jìn)行1000次獨(dú)立重復(fù)試驗(yàn)。實(shí)驗(yàn)結(jié)果如圖3所示。

從圖3可以看到在有異常點(diǎn)和相同的初始值的情況下，加權(quán)擴(kuò)展正交迭代算法對(duì)于旋轉(zhuǎn)矩陣和平移向量的計(jì)算精度，相比于擴(kuò)展正交迭代和正交迭代算法有較大提高并且保持在較高的水平。

2.1.2 異常點(diǎn)和高斯噪聲實(shí)驗(yàn)

異常點(diǎn)和高斯噪聲實(shí)驗(yàn)（研究存在異常參考點(diǎn)時(shí)算法計(jì)算精度和高斯噪聲的關(guān)系）：參考點(diǎn)數(shù)目固定為10，參考直線數(shù)目為2，高斯噪聲標(biāo)準(zhǔn)差統(tǒng)一從1pixel增加到10pixel，每次隨機(jī)選擇一個(gè)參考點(diǎn)加上標(biāo)準(zhǔn)差為當(dāng)前標(biāo)準(zhǔn)差5倍的高斯噪聲。對(duì)于不同的標(biāo)準(zhǔn)差都進(jìn)行1000次獨(dú)立重復(fù)試驗(yàn)。實(shí)驗(yàn)結(jié)果如圖4所示。

從圖4可以看到隨著高斯噪聲水平的增加，加權(quán)擴(kuò)展正交迭代的計(jì)算精度始終最高，相比于沒有考慮加權(quán)系數(shù)的擴(kuò)展正交迭代有較大提高。同時(shí)可以看出只考慮深度信息的加權(quán)擴(kuò)展正交迭代計(jì)算精度高于沒有加權(quán)的擴(kuò)展正交迭代，只考慮初始值的重投影誤差的擴(kuò)展正交迭代對(duì)異常點(diǎn)的抗干擾性較強(qiáng)。

2.1.3 平移距離實(shí)驗(yàn)

平移距離實(shí)驗(yàn)（研究存在異常點(diǎn)和高斯噪聲時(shí)計(jì)算精度和平移距離的關(guān)系）：固定參考點(diǎn)數(shù)目為10，參考直線數(shù)目為2，隨機(jī)一個(gè)參考點(diǎn)加上為10pixel的高斯噪聲，其余參考點(diǎn)加上為1pixel的高斯噪聲，令平移向量和在[－20, 20]（步長(zhǎng)5乘上參考點(diǎn)區(qū)域大小4）的區(qū)域內(nèi)隨機(jī)產(chǎn)生，令參考點(diǎn)區(qū)域到相機(jī)距離與參考點(diǎn)區(qū)域大小之比/4以步長(zhǎng)為5從5增加到40，每次做1000次獨(dú)立重復(fù)性實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如圖5所示。

圖3 參考點(diǎn)數(shù)目增加時(shí)，旋轉(zhuǎn)矩陣和平移向量的相對(duì)計(jì)算誤差

圖4 高斯噪聲水平增加時(shí)，旋轉(zhuǎn)矩陣和平移向量的相對(duì)計(jì)算誤差

從圖5可以看到隨著距離的增加，加權(quán)擴(kuò)展正交迭代的計(jì)算精度始終最高，且受距離的影響較小。

2.1.4 計(jì)算時(shí)間實(shí)驗(yàn)

計(jì)算時(shí)間實(shí)驗(yàn)（研究算法的計(jì)算速度隨參考點(diǎn)數(shù)目的變化）：參考點(diǎn)數(shù)目從4增加到104，參考直線數(shù)目為2，在參考點(diǎn)上加上標(biāo)準(zhǔn)差為3pixel的高斯噪聲，計(jì)算1000次獨(dú)立重復(fù)試驗(yàn)算法消耗時(shí)間的平均值，實(shí)驗(yàn)結(jié)果如圖6所示。

從圖6可以看出，沒有加速的加權(quán)擴(kuò)展正交迭代的計(jì)算時(shí)間遠(yuǎn)多于擴(kuò)展正交迭代和正交迭代算法，而加速后的加權(quán)擴(kuò)展正交迭代算法則大量節(jié)省了計(jì)算時(shí)間，大約可以減少40ms。在參考點(diǎn)增多以后，因?yàn)樾枰獙?duì)每個(gè)參考點(diǎn)和參考直線段端點(diǎn)加以權(quán)重的原因，計(jì)算時(shí)間會(huì)多于沒有加速的正交迭代算法，但是少于沒有加速的加權(quán)正交迭代。

2.2 實(shí)物實(shí)驗(yàn)

在真實(shí)圖像實(shí)驗(yàn)中，使用KITTI[16]（Karlsruhe Institute of Technology and Toyota Technological Institute at Chicago）標(biāo)準(zhǔn)數(shù)據(jù)集對(duì)PnP算法進(jìn)行驗(yàn)證。KITTI數(shù)據(jù)集是由德國(guó)卡爾斯魯勒理工學(xué)院搭建，包括了城市、鄉(xiāng)村、高速公路等多種環(huán)境，通過自主駕駛汽車進(jìn)行采集，車上載有多種傳感器，其中高精度的組合導(dǎo)航系統(tǒng)為數(shù)據(jù)集提供相機(jī)位姿真值。數(shù)據(jù)集中的圖片分辨率為1226pixel×370pixel，相機(jī)等效焦距為707.0912pixel。

在實(shí)驗(yàn)中首先對(duì)序列圖片進(jìn)行特征點(diǎn)匹配和直線段匹配，直線段檢測(cè)使用LSD[17]算法（line segment detector），得到特征點(diǎn)和直線段端點(diǎn)的坐標(biāo)，其中直線匹配結(jié)果如圖7所示。

圖5 相機(jī)與參考點(diǎn)區(qū)域距離增加時(shí)，旋轉(zhuǎn)矩陣和平移向量的相對(duì)計(jì)算誤差

圖6 參考點(diǎn)數(shù)目增加時(shí)，各種算法的計(jì)算時(shí)間對(duì)比

初始化時(shí)通過對(duì)極幾何計(jì)算第一幀和第二幀之間的旋轉(zhuǎn)和平移矩陣，通過三角化計(jì)算匹配的特征點(diǎn)和直線端點(diǎn)的三維坐標(biāo)，然后利用三維坐標(biāo)和特征點(diǎn)和直線段點(diǎn)在下一幀的二維坐標(biāo)計(jì)算相機(jī)的旋轉(zhuǎn)和平移。初始化后則直接利用上一次PnP算法計(jì)算的旋轉(zhuǎn)和平移矩陣，通過三角化來計(jì)算三維坐標(biāo)以進(jìn)行下一次的PnP。實(shí)驗(yàn)結(jié)果如圖8，9所示。

從圖8，9中可以看到在序列圖片實(shí)驗(yàn)中本文提出算法的計(jì)算誤差遠(yuǎn)小于傳統(tǒng)正交迭代算法的誤差，計(jì)算準(zhǔn)確性較高。由于直線提取以及匹配的不穩(wěn)定性，在個(gè)別圖像中加速加權(quán)擴(kuò)展正交迭代算法的計(jì)算誤差大于傳統(tǒng)正交迭代算法，但是仍保持在較低水平，并且這種情況出現(xiàn)的次數(shù)較少，不影響整體計(jì)算的準(zhǔn)確性。在序列圖像中的實(shí)驗(yàn)證明了提出的算法有較高的準(zhǔn)確性和魯棒性，充分利用了圖像中的信息，在實(shí)際場(chǎng)景應(yīng)用中有較高價(jià)值。

圖7 序列圖像直線匹配結(jié)果，相匹配的直線用同樣的顏色和相同的序號(hào)標(biāo)注

圖8 序列5中旋轉(zhuǎn)矩陣和平移向量的計(jì)算誤差

圖9 序列6中旋轉(zhuǎn)矩陣和平移向量的計(jì)算誤差

由于在序列圖片中每次進(jìn)行位姿估計(jì)時(shí)，需要根據(jù)上一次的計(jì)算結(jié)果來使用PnP算法計(jì)算本次的位姿，使用快速加權(quán)擴(kuò)展正交迭代算法會(huì)有一定的累積誤差，但是由于提取了序列圖片中的直線信息并且對(duì)參考點(diǎn)和線段端點(diǎn)加以權(quán)重，該算法的累積誤差遠(yuǎn)小于傳統(tǒng)的正交迭代算法的累積誤差。對(duì)于位姿計(jì)算實(shí)驗(yàn)中的累積誤差，可以使用光束平差法[5]對(duì)位姿進(jìn)行優(yōu)化從而抑制累積誤差，同時(shí)可以使用回環(huán)檢測(cè)[5]的方法對(duì)位姿進(jìn)行優(yōu)化，從而在一定程度上消除累積誤差。

3 結(jié)論

針對(duì)二維—三維特征對(duì)應(yīng)（2D-3D）的相機(jī)姿態(tài)估計(jì)算法（PnP）進(jìn)行了研究，在基于點(diǎn)和直線段的擴(kuò)展正交迭代算法的基礎(chǔ)上提出了快速加權(quán)擴(kuò)展正交迭代算法。該算法可以利用圖像中的直線信息，既考慮了參考點(diǎn)以及參考直線端點(diǎn)的深度信息，又考慮了參考點(diǎn)和參考直線端點(diǎn)的重投影誤差，并通過公式推導(dǎo)將每一次迭代的時(shí)間復(fù)雜度從()降到了(1)，從而提高了算法的計(jì)算精度和計(jì)算速度。

數(shù)值仿真實(shí)驗(yàn)和真實(shí)圖片實(shí)驗(yàn)，對(duì)所提出的位姿估計(jì)算法進(jìn)行了驗(yàn)證，并與正交迭代算法和擴(kuò)展正交迭代算法進(jìn)行對(duì)比，結(jié)果證明該算法有效地提高了計(jì)算精度、計(jì)算速度，對(duì)異常點(diǎn)有較強(qiáng)的抗干擾能力，對(duì)復(fù)雜環(huán)境下的實(shí)時(shí)相機(jī)姿態(tài)估計(jì)問題具有一定價(jià)值。

[1] Mullen R.[M]. Barlow: Asprs Publications, 2004.

[2] Hartley R, Zisserman A.[M]. Cambridge: Cambridge university press, 2003.

[3] Gingold Y, Igarashi T, Zorin D. Structured annotations for 2D-to-3D modeling[C]//()., 2009, 28(5): 148.

[4] DeSouza G N, Kak A C. Vision for mobile robot navigation: A survey[J]., 2002, 24(2): 237-267.

[5] Mur-Artal R, Montiel J M M, Tardos J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]., 2015, 31(5): 1147-1163.

[6] 錢萍, 王惠南. 基于對(duì)偶四元數(shù)的航天器交會(huì)對(duì)接位姿雙目視覺測(cè)量算法[J]. 宇航學(xué)報(bào), 2013, 34(1): 32-38.

QIAN Ping, WANG Huinan. A Binocular Vision Algorithm Based on Dual Quaternion for Relative Position and Attitude Determinati of RVD Spacecrafts[J]., 2013, 34(1): 32-38.

[7] 吳凡路, 劉建軍, 任鑫, 等. 基于圓形標(biāo)志點(diǎn)的深空探測(cè)全景相機(jī)標(biāo)定方法[J]. 光學(xué)學(xué)報(bào), 2013, 33(11): 1115002.

WU Fanlu, LIU Jianjun, REN Xin, et al. Deep space exploration panoramic camera calibration technique basedon circular markers[J]., 2013, 33(11): 1115002.

[8] Azuma R, Baillot Y, Behringer R, et al. Recent advances in augmented reality[J]., 2001, 21(6): 34-47.

[9] Haralick R M, Joo H, Lee C N, et al. Pose estimation from corresponding point data[J]., 1989, 19(6): 1426-1446.

[10] LU C P, Hager G D, Mjolsness E. Fast and globally convergent pose estimation from video images[J]., 2000, 22(6): 610-622.

[11] Pumarola A, Vakhitov A, Agudo A, et al. PL-SLAM: Real-time monocular visual SLAM with points and lines[C]//2017()., 2017: 4503-4508.

[12] 許允喜, 蔣云良, 陳方. 基于點(diǎn)和直線段對(duì)應(yīng)的擴(kuò)展正交迭代位姿估計(jì)算法[J]. 光學(xué)學(xué)報(bào), 2009(11): 3129-3135.

XU Yunxi, JIANGYunliang, CHEN Fang. An Extended Orthogonal Iteration Algorithm for Pose Estimation from Point and Line Segment Correspondences [J]., 2009, 29(11): 3129-3135.

[13] 周潤(rùn), 張征宇, 黃敘輝. 相機(jī)位姿估計(jì)的加權(quán)正交迭代算法[J]. 光學(xué)學(xué)報(bào), 2018, 38(5): 0515002.

ZHOU Run, ZHANG Zhengyu, HUANG Xuhui. Weighted Orthogonal Iteration Algorithm for Camera Pose Estimation[J]., 2018, 38(5): 193-199.

[14] ZHENG Y, KUANG Y, Sugimoto S, et al. Revisiting the pnp problem: A fast, general and optimal solution[C]//, 2013: 2344-2351.

[15] LI S, XU C, XIE M. A robust O (n) solution to the perspective-n-point problem[J]., 2012, 34(7): 1444-1450.

[16] Geiger A, Lenz P, Stiller C, et al. Vision meets robotics: The KITTI dataset[J]., 2013, 32(11): 1231-1237.

[17] VonGioi R G, Jakubowicz J, Morel J M, et al. LSD: A fast line segment detector with a false detection control[J]., 2008, 32(4): 722-732.

Accelerativeand Weighted Camera Pose Estimation Based on Extended Orthogonal Iterative Algorithm

WANG Shi’an，WANG Xiangjun，YIN Lei

(State Key Laboratory of Precision Measurement Technology and Instruments, Tianjin University, Tianjin 300072, China)

The camera pose estimation algorithm is typically more based on reference points and less on the linear information in the image. In this paper, based on the extended orthogonal iteration, an accelerative and weighted camera pose estimation algorithm based on the combination of point and line segments is proposed to ensure a good anti-interference and real-time performance of the algorithm. The algorithm considers the sum of the weighted collinearity error and the weighted coplanarity error as the error function. The weight coefficients are determined by the depth information and the re-projection error of the initial value. In addition, the algorithm accelerates the iterative process and reduces the time complexity of the iteration process dramatically from() to(1). The simulation results demonstrate that the algorithm can suppress the interference of abnormal points and reduce the calculation time. The calculation accuracy of the rotation matrix is 51.69% higher than that of the traditional orthogonal iterative algorithm, and the calculation accuracy of the translation vector is increased by 51.21%. Furthermore, the calculation time after the acceleration is reduced by 47.11%. The physical experiment shows that the algorithm can make full use of the detected reference points and reference line information, improve the calculation accuracy, reduce the cumulative error, and has high practical application value.

machine vision, image processing, pose estimation, iterative algorithms, computational complexity

TP391

A

1001-8891(2020)05-0205-08

2019-10-09；

2020-02-27.

王世安（1995-），男，碩士，主要研究方向?yàn)橛?jì)算機(jī)視覺和圖像處理。E-mail：sean_wang@tju.edu.cn。