基于擴展正交迭代的快速加權的相機位姿估計

王世安，王向軍，陰 雷

〈系統與設計〉

基于擴展正交迭代的快速加權的相機位姿估計

王世安，王向軍，陰 雷

（天津大學 精密測試技術及儀器國家重點實驗室，天津 300072）

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

機器視覺；圖像處理；位姿估計；迭代算法；計算復雜度

0 引言

相機位姿估計問題是攝影測量學[1]、計算機視覺[2]、計算機圖形學[3]以及機器人科學[4]的一個重要問題，在無人平臺導航[5]、航天器對接[6]、相機標定[7]、增強現實[8]等領域有廣泛應用。位姿估計問題根據相對應特征信息的不同可以分為3類[9]：2D-2D位姿估計、2D-3D位姿估計、3D-3D位姿估計。其中2D-3D位姿估計問題又叫做點透視問題（PnP，Perspective-n- Point）。

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

1 基于點和直線段結合的快速加權正交迭代算法

1.1 擴展正交迭代算法

正交迭代算法[10]計算參考點的共線性誤差，并以此作為迭代計算的誤差函數，即：

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

圖1 物體空間和歸一化平面上的對應直線段

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

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

設位姿估計前可以提取到個參考點(1,2, …,p)和個參考直線段(1,2, …,L)，則擴展正交迭代算法的目標函數為：

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

1.2 加權擴展正交迭代

1.2.1 加權擴展正交迭代算法

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

式中：＞0為第個參考點（包括直線段的兩個端點）的權重系數。加入權重后，當給定第次對于的估計值(k)以后，可以得出此時的最優值為：

1.2.2 權重系數確定方法

式中：為相機內參矩陣。

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

1.3 快速加權擴展正交迭代

正交迭代算法作為非線性PnP算法計算時間較長，在對每個參考點加入權值以及擴展到直線段以后又增加了較大的計算量，計算消耗時間進一步增加，本文通過將迭代算法公式中的矩陣拉直，并通過整合使加權擴展正交迭代算法每次迭代的計算復雜度從()降到了(1)，具體推導過程如下：

首先引入公式：

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

式中：

進一步可得：

式中：

整理公式得：

對進行拉直運算可以得到：

式中：

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

在每次迭代過程中對誤差函數進行規整可得：

式中：

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

2 實驗

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

2.1 仿真實驗

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

2.1.1 參考點數目實驗

參考點數目實驗（研究存在異常參考點時算法計算精度和參考點數目的關系）：參考點數目從4增加到12，參考直線數目為2，隨機一個參考點加上為5pixel的高斯噪聲，其余參考點加上為0.5pixel的高斯噪聲。對于不同的參考點數目都進行1000次獨立重復試驗。實驗結果如圖3所示。

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

2.1.2 異常點和高斯噪聲實驗

異常點和高斯噪聲實驗（研究存在異常參考點時算法計算精度和高斯噪聲的關系）：參考點數目固定為10，參考直線數目為2，高斯噪聲標準差統一從1pixel增加到10pixel，每次隨機選擇一個參考點加上標準差為當前標準差5倍的高斯噪聲。對于不同的標準差都進行1000次獨立重復試驗。實驗結果如圖4所示。

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

2.1.3 平移距離實驗

平移距離實驗（研究存在異常點和高斯噪聲時計算精度和平移距離的關系）：固定參考點數目為10，參考直線數目為2，隨機一個參考點加上為10pixel的高斯噪聲，其余參考點加上為1pixel的高斯噪聲，令平移向量和在[－20, 20]（步長5乘上參考點區域大小4）的區域內隨機產生，令參考點區域到相機距離與參考點區域大小之比/4以步長為5從5增加到40，每次做1000次獨立重復性實驗，實驗結果如圖5所示。

圖3 參考點數目增加時，旋轉矩陣和平移向量的相對計算誤差

圖4 高斯噪聲水平增加時，旋轉矩陣和平移向量的相對計算誤差

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

2.1.4 計算時間實驗

計算時間實驗（研究算法的計算速度隨參考點數目的變化）：參考點數目從4增加到104，參考直線數目為2，在參考點上加上標準差為3pixel的高斯噪聲，計算1000次獨立重復試驗算法消耗時間的平均值，實驗結果如圖6所示。

從圖6可以看出，沒有加速的加權擴展正交迭代的計算時間遠多于擴展正交迭代和正交迭代算法，而加速后的加權擴展正交迭代算法則大量節省了計算時間，大約可以減少40ms。在參考點增多以后，因為需要對每個參考點和參考直線段端點加以權重的原因，計算時間會多于沒有加速的正交迭代算法，但是少于沒有加速的加權正交迭代。

2.2 實物實驗

在真實圖像實驗中，使用KITTI[16]（Karlsruhe Institute of Technology and Toyota Technological Institute at Chicago）標準數據集對PnP算法進行驗證。KITTI數據集是由德國卡爾斯魯勒理工學院搭建，包括了城市、鄉村、高速公路等多種環境，通過自主駕駛汽車進行采集，車上載有多種傳感器，其中高精度的組合導航系統為數據集提供相機位姿真值。數據集中的圖片分辨率為1226pixel×370pixel，相機等效焦距為707.0912pixel。

在實驗中首先對序列圖片進行特征點匹配和直線段匹配，直線段檢測使用LSD[17]算法（line segment detector），得到特征點和直線段端點的坐標，其中直線匹配結果如圖7所示。

圖5 相機與參考點區域距離增加時，旋轉矩陣和平移向量的相對計算誤差

圖6 參考點數目增加時，各種算法的計算時間對比

初始化時通過對極幾何計算第一幀和第二幀之間的旋轉和平移矩陣，通過三角化計算匹配的特征點和直線端點的三維坐標，然后利用三維坐標和特征點和直線段點在下一幀的二維坐標計算相機的旋轉和平移。初始化后則直接利用上一次PnP算法計算的旋轉和平移矩陣，通過三角化來計算三維坐標以進行下一次的PnP。實驗結果如圖8，9所示。

從圖8，9中可以看到在序列圖片實驗中本文提出算法的計算誤差遠小于傳統正交迭代算法的誤差，計算準確性較高。由于直線提取以及匹配的不穩定性，在個別圖像中加速加權擴展正交迭代算法的計算誤差大于傳統正交迭代算法，但是仍保持在較低水平，并且這種情況出現的次數較少，不影響整體計算的準確性。在序列圖像中的實驗證明了提出的算法有較高的準確性和魯棒性，充分利用了圖像中的信息，在實際場景應用中有較高價值。

圖7 序列圖像直線匹配結果，相匹配的直線用同樣的顏色和相同的序號標注

圖8 序列5中旋轉矩陣和平移向量的計算誤差

圖9 序列6中旋轉矩陣和平移向量的計算誤差

由于在序列圖片中每次進行位姿估計時，需要根據上一次的計算結果來使用PnP算法計算本次的位姿，使用快速加權擴展正交迭代算法會有一定的累積誤差，但是由于提取了序列圖片中的直線信息并且對參考點和線段端點加以權重，該算法的累積誤差遠小于傳統的正交迭代算法的累積誤差。對于位姿計算實驗中的累積誤差，可以使用光束平差法[5]對位姿進行優化從而抑制累積誤差，同時可以使用回環檢測[5]的方法對位姿進行優化，從而在一定程度上消除累積誤差。

3 結論

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

數值仿真實驗和真實圖片實驗，對所提出的位姿估計算法進行了驗證，并與正交迭代算法和擴展正交迭代算法進行對比，結果證明該算法有效地提高了計算精度、計算速度，對異常點有較強的抗干擾能力，對復雜環境下的實時相機姿態估計問題具有一定價值。

[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] 錢萍, 王惠南. 基于對偶四元數的航天器交會對接位姿雙目視覺測量算法[J]. 宇航學報, 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] 吳凡路, 劉建軍, 任鑫, 等. 基于圓形標志點的深空探測全景相機標定方法[J]. 光學學報, 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] 許允喜, 蔣云良, 陳方. 基于點和直線段對應的擴展正交迭代位姿估計算法[J]. 光學學報, 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] 周潤, 張征宇, 黃敘輝. 相機位姿估計的加權正交迭代算法[J]. 光學學報, 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-），男，碩士，主要研究方向為計算機視覺和圖像處理。E-mail：sean_wang@tju.edu.cn。