融合二維圖像和三維點云的相機位姿估計

周佳樂，朱兵，吳芝路

（哈爾濱工業大學 電子與信息工程學院，黑龍江 哈爾濱 150001）

1 引言

相機位姿估計是機器人應用、自主導航以及增強現實中的關鍵技術，其目的是通過當前相機所采集的圖像信息，以及已知環境的先驗信息，來估計出當前相機設備在三維真實場景中的位置和姿態，由于相機的位姿信息包含六個自由度，因此也稱為相機的六自由度估計。隨著自動駕駛、精確定位導航等領域對定位的精度要求不斷提高，僅僅使用二維圖像已經無法對現實的三維世界進行準確描述，因此包含深度信息的三維場景點云成為了定位技術更為關注的重點，通過三維場景點云以及圖像處理的相關算法可以實現更加精確的應用，這也成為了相機位姿估計技術的一個發展方向。

近年，相機位姿估計技術在國內外發展迅速，傳統的相機位姿估計方法首先采用SFM（Structure From Motion）算法對圖像數據庫構建三維點云數據，同時將每一個3D點云與特征算子相關聯。對于查詢圖像，提取其特征描述子并與三維點云匹配，使用PnP（Perspective-n-Point）位姿求解算法解得最終的位姿［1-4］。這種方法稱為基于結構的相機位姿估計。然而這種方法存在較大局限，一旦點云與特征描述符的關系確定，對于位姿查詢圖像，只能提取特定的特征描述符來進行2D-3D匹配，這種構建2D-3D匹配點的思路在關聯特征描述符不易提取的情況下表現往往較差。因此本文結合圖像檢索的思路與特征匹配算法，充分利用深度圖像信息，在2D-3D匹配點構建思路上進行改進；同時提出位姿優化目標函數，對PnP位姿解算的結果進行進一步優化，提出多階段的相機位姿估計方法。

隨著深度學習方法的廣泛應用，大量學者投身于采用深度學習的方式來解決相機位姿估計問 題。Kendall等 人［5］將GoogleNet網 絡 進 行 改進，使用神經網絡直接預測相機的六自由度位姿，然而該方法最終的相機位姿估計精度較低，文獻［6］進一步證明了直接使用神經網絡估計相機位姿的方法準確率只能逼近使用圖像檢索的方法［7］。Li等人［8］通過孿生神經網絡來擬合查詢圖像與參考圖像之間的相對位姿關系，進一步實現相機的位姿估計，由于圖像中并不包含場景的三維場景信息，因此單獨使用圖像進行位姿估計的誤差較大。Tang等人［9］利用深度檢索算法得到參考圖像，在查詢圖像與參考圖像之間構建一個損失向量訓練神經網絡并獲得稠密場景匹配，進一步實現相機位姿估計，其本質是采用神經網絡擬合查詢圖像與參考圖像的關系，因此前端圖像檢索中存在較大誤差時，其位姿估計精度往往較差?？傊?，目前將神經網絡模型直接用于相機位姿估計的端到端學習方式，由于網絡模型參數的有限性，無法完全擬合所有的三維場景信息，導致其位姿估計結果的精度較低或速度較差。為實現更加精確且高效的相機位姿估計方法，本文將深度學習算法用于傳統基于結構的相機位姿估計算法當中，設計卷積神經網絡用于獲得更為稠密的2D-3D匹配點，提出基于稠密場景回歸的多階段相機空間位姿估計方法，以此提高相機位姿估計算法的精確性。

2 數據集的構建

不同的真實場景對相機位姿估計算法精度的影響較大，為推進特定場景下相機位姿估計算法的研究，本文首先構建相機位姿估計數據集。

2.1 像素坐標與世界坐標關系

本文基于二維圖像和三維點云對單目相機進行空間位姿估計，數據集中包含了單目相機的RGB圖像以及RGB圖像所對應的三維空間坐標信息。在三維圖像處理中要考慮到2個三維坐標系和2個二維坐標系。這4個坐標系互相嵌套，相機坐標系以世界坐標系為基礎，圖像坐標系以相機坐標系為基礎，像素坐標系則以圖像坐標系為基礎。世界坐標系一旦確定則不會變化，而相機坐標系一直相對世界坐標系發生變換，圖像坐標系則相對相機坐標系保持穩定不變。圖1為各個坐標系之間的關系：

圖1 各坐標系間的關系Fig.1 Relationship between coordinate systems

圖中，Ow-Xw，Yw，Zw為世界坐標系，原點可以認為是相機的真實世界坐標；Oc-Xc，Yc，Zc為相機坐標系，原點為相機的光心；o-x，y為圖像坐標系。

3個坐標系的變換關系可以通過外參矩陣以及內參矩陣來進行描述，外參矩陣是相機相對起始點的位置與姿勢的變化，用來描述世界坐標系和相機坐標系之間的轉換關系；內參矩陣是相機本身的固定參數，是指相機坐標系下的物體投影到圖像平面下的內部參數，通常用來描述相機坐標系和圖像坐標系之間的關系。通過坐標系關系轉換，可以得到像素坐標系與世界坐標系的關系為：

在這種轉換關系中，內參矩陣可以通過相機標定的方式精準獲得，在相機應用環境不發生變化時，可認為其不發生變化。然而，在相機位姿發生變化時，其外參矩陣也將發生變化。本文的目標是通過模型估計出當前相機的外參矩陣，因此數據集中包括相機所獲得的RGB圖像（像素坐標）、RGB圖像所對應的相機外參矩陣以及相機所處場景的三維真實場景信息（世界坐標）。

2.2 SFM三維重建

通過分析像素坐標和世界坐標之間的關系，可以知道構建數據集的關鍵是獲得單張RGB圖像的外參矩陣以及相機所處場景的三維真實場景。本文采用SFM算法對相機所拍攝場景進行三維重建，并獲得重建后每一張RGB圖在三維場景中的外參信息。SFM算法是一種基于各種收集到的無序圖片進行三維重建的離線算法，通過相機的移動采集不同視角的圖像，求解相機的相對位姿信息并恢復出場景的三維信息。通過SFM算法，可以求解得到每一張RGB圖像的三維稀疏點云數據以及其相對于該三維稀疏點云下的位姿信息，圖2是采用該算法進行三維重建的效果示意圖（彩圖見期刊電子版），圖中紅色軌跡為每一張RGB圖像所對應的真實相機位姿，黑色點為三維稀疏點云：

圖2 三維稀疏點云重建及位姿數據集構建Fig.2 3D sparse point cloud reconstruction and pose dataset construction

基于SFM算法進行三維重建的三維點云是由特征匹配所提供的，由于這些匹配點的提取是稀疏的，因此采用SFM算法進行三維重建得到的點云數據也將是稀疏的。為了研究更加精準的位姿估計算法，本文在構建數據集的時還需產生稠密的點云數據。SFM算法可以解算得到每一幀RGB圖像在當前三維稀疏點云下的位姿矩陣，為得到圖像的稠密三維坐標信息，還需要采用深度相機同步采集每一幀RGB圖像的深度信息［10］，通過前述三維重建過程中得到的位姿以及坐標間的變換關系，求解出場景的稠密三維點云。本文實驗使用Astra Mini深度相機作為RGB圖像以及深度圖像采集設備。經過實際標定實驗，該深度相機的深度有效測量范圍為0.6～8 m，其精度為±(1～3)mm/m，深度圖像數據為16位深度值，其深度值為相機與物體的實際距離，單位為mm。由于實驗中不可避免存在無效的測量深度值（深度圖像中黑色邊緣部分），在實際實驗過程中，將有效深度值對RGB圖像進行掩摸處理，舍棄無效的深度值。本文對實驗室進行采集，最終重建的結果如圖3所示。

圖3 稠密點云數據構建Fig.3 Dense point cloud data construction

通過以上方法，構建得到最終的數據集，該數據集包含4 000張RGB圖像、深度圖以及每張RGB圖像對應的位姿信息，本文采集的數據為連續幀圖像，將圖像以1 000幀為單位劃分為4個序列，其中1，3序列作為訓練集，2，4序列作為測試集，具體信息如表1所示。

表1 Labcore數據集Tab.1 Detail of labcore dataset

3 多階段相機位姿估計

傳統基于結構的相機位姿估計方法是找到2D-3D匹配點，通過PnP位姿求解算法求解方程，以解算得到當前相機的具體位姿［1-4］，該方法在構建點云數據時將3D點與特定的2D特征點進行關聯，一旦數據構建完成，其特征描述符便無法更換，而不同場景下采用不同特征描述符所得的位姿估計結果差異較大，這也成為其在特征描述符不易提取的環境中表現較差的主要原因。因此本文首次將圖像檢索算法引入2D-3D匹配點的構建當中，并采用傳統基于結構算法的相機位姿估計思路，同時構建目標函數優化位姿估計結果，提出基于多階段的相機位姿估計方法，該方法結合圖像檢索方法與基于結構相機位姿估計方法的優勢，提升了2D-3D匹配點的匹配速度與精度，具體過程為：第一階段，構建2D-3D匹配點，采用深度圖像檢索算法檢索出查詢圖像的最近鄰幀作為參考圖像，通過特征匹配算法及坐標映射關系找到用于位姿解算的2D-3D匹配點；第二階段，粗位姿求解并剔除外點，使用PnP位姿求解算法求解相機位姿，對求解得到的位姿進行評分，采用隨機抽樣一致（Random Sample Consensus，RANSAC）算法的思路剔除誤匹配點，得到初始位姿估計；第三階段，位姿對齊優化，將前一階段求解的位姿作為初始位姿，設計目標函數，用三維點云將查詢圖像位姿與參考圖像位姿對齊，采用最小二乘法對初始位姿估計結果進行進一步優化，得到最佳位姿估計結果。圖4是整個算法的流程圖。

圖4 多階段相機位姿估計方法流程Fig.4 Flow of multistage camera pose estimation

3.1 構建2D-3D匹配點

為檢索得到查詢圖像的最近鄰圖像（與查詢圖像真實位姿最接近的圖像），本文采用卷積神經網絡對查詢圖像以及已知位姿圖像數據庫的全局特征進行提取。采用VGG網絡結構進行卷積神經網絡的搭建，為了使得深度圖像檢索算法能夠適應大部分的圖像場景，將VGG網絡在ImageNet數據集上進行預訓練，通過一個全連接層對圖像數據庫以及查詢圖像的每一張圖像提取出一個1×512的圖像全局特征向量。在圖像檢索階段，將圖像數據庫中與查詢圖像相似度最高的圖像作為參考圖像。

完成最近鄰幀檢索之后，對查詢圖像及參考圖像進行特征匹配以進一步找到2D-3D匹配點。為了進一步分析不同特征描述子對特征匹配結果及最終的位姿解算結果的影響，本文分別采用ORB（Oriented Fast and Rotated Brief）算 子、SIFT（Scale Invariant Feature Transform）算子以及SURF（Speeded-Up Robust Features）算 子［11］對圖像的特征進行提取，并使用暴力匹配法進行匹配。

3.2 粗位姿求解并剔除外點

使用特征匹配算法對查詢圖像及參考圖像的特征點進行匹配，并計算像素和世界坐標間的坐標映射關系獲得2D-3D匹配點，利用PnP算法對2D-3D匹配點進行位姿解算得到當前相機具體位姿。然而實際上特征匹配算法的匹配結果中可能存在誤匹配點，導致最終使用PnP算法進行位姿求解時的結果誤差較大，為了進一步剔除誤 匹 配 點，本 文 采 用RANSAC算 法［1-3，13］的思 想對相機位姿估算的結果進行進一步的優化。圖5是采用RANSAC算法思想進行位姿估計結果優化的流程圖。

圖5 RANSAC算法流程Fig.5 Flow of RANSAC algorithm

模型估計：在特征匹配算法得到的2D-3D特征匹配點中選取一定數量的點對作為PnP求解問題的輸入，并采用PnP求解算法解得相機位姿假設。

模型評價：為了判斷樣本數據點是樣本內點還是樣本外點，定義重投影誤差作為判斷依據，重投影誤差的定義如式（2）：

式中：pi為查詢二維圖像中第i個像素在圖像中的2D位置，C為相機的內參矩陣，hJ為相機位姿假設數學模型，yi為pi對應的三維場景坐標真實值。由此，設置誤差閾值τ，本文實驗中τ=5，如果ei＜τ，則稱對應點對pi-yi為內點；反之，對應點pi-yi為外點。記錄每個數學模型的內點數量作為模型的評分s(hJ)。

模型篩選：重復以上步驟直至抵達迭代次數（本文實驗設置為10 000次）上限或所有匹配點的重投影誤差均小于預設閾值。統計不同的位姿假設數學模型下的樣本內點數量，樣本內點數量最多，即模型的評分最大的模型作為最佳數學模型，保留所有樣本內點，剔除樣本外點，并將剔除外點后的對應點對用于下一步的點云配準操作。

3.3 位姿對齊優化

位姿優化的目標是尋找最優的相機估計位姿(R?，T?)，使得估計的位姿矩陣與真實位姿矩陣的誤差達到最小。光束平差法（Bundle Adjustment，BA）是相機位姿優化領域的一個經典方法，該方法在大量文獻［11-16］中的應用也已證明其位姿優化的有效性。因此本文參考文獻［16］的優化目標函數，設計目標函數構建一個光束平差問題來優化第二階段中相機位姿估計結果，使得三維點云對于查詢圖像q與參考圖像r的重投影差異最小化，定義的目標函數如式（3）所示：

式中：piq=ChJ yi為參考圖像r視場范圍內的第i個3D點yi在當前相機位姿估計下的重投影坐標值，[?]為亞像素級別插值，本文實驗過程中采用線性插值，Iq為當前像素點的歸一化像素值。在歐式群SE（3）中，采用對應的李代數對位姿更新δ∈R6進行參數化［13］。本文采用高斯牛頓法來解決上述最小二乘問題，雅克比矩陣和黑塞矩陣計算如式（4）所示：

通過雅克比矩陣和黑塞矩陣計算位姿更新：

則最終的位姿更新為：

采用上述過程對估計的相機位姿(?)進行更新，直到位姿更新量δ足夠小時停止更新，本文 實 驗 中 當dT＜0.005，dR＜0.05時 停 止更新。

4 基于稠密場景回歸的多階段相機位姿估計

4.1 稠密場景回歸

第3節中所述采用深度圖像檢索及特征匹配算法來確定2D-3D匹配點，進一步實現相機的空間位姿估計。然而當圖像中特征點不易提取以及查詢圖像與參考圖像場景變化較大的情況下，該方法存在大量的誤匹配點，導致最終的位姿解算結果誤差較大甚至出現錯誤。同時由于特征匹配點較少，當存在大量誤匹配點時，通過RANSAC算法剔除了異常點［17］，便無法得到足量用于PnP解算的2D-3D匹配點。然而特征點在每一幀圖像像素點中占比其實是較少的，即提取的特征點相對與圖像像素點來說是稀疏的，獲得的2D-3D匹配點也是稀疏的。為了獲得稠密的2D-3D匹配點，以此來提高相機空間位姿估計的精度，本文采用卷積神經網絡對輸入的RGB圖像進行場景坐標回歸，構建稠密的2D-3D匹配點，以實現第3節所述多階段相機位姿估計方法中的第一階段2D-3D匹配點的構建，提出基于稠密場景回歸的多階段相機位姿估計方法。

本文采用ResNet神經網絡結構，在保證網絡深度的同時防止網絡的退化。為實現相機稠密坐標回歸并減少計算量，對ResNet網絡結構進行改進，將全連接層刪除，構建全卷積神經網絡，通過卷積層輸出一個3通道的特征圖作為最終的預測結果，網絡輸出為160×120的三維場景坐標回歸，即是輸入圖像下采樣后的三維場景坐標。圖6是本文構建的稠密場景回歸網絡結構。

圖6 稠密場景回歸網絡結構Fig.6 Structure dense scene regression network

采用以上神經網絡結構對查詢圖像的三維坐標值進行回歸，首先通過4個卷積層對圖像的特征進行提取，之后經過3個殘差網絡結構對場景回歸進行更好地擬合，通過卷積層輸出最終的三維坐標預測值。本文的Loss函數定義如公式（7）［18］：

式中：gt_coords表示查詢圖像每一個像素區域對應的三維坐標真實值，pred_coords表示網絡模型對查詢圖像每一個像素區域（4×4像素點區域）三維坐標的預測值，Norm(·)為歸一化函數，s為損失因子，i表示第i個像素區域，n為像素區域總數。

4.2 場景回歸結果

本文采用訓練完成后的網絡對數據集下的測試圖像進行場景回歸。本文將真實場景坐標以及預測的場景坐標的三維信息（x，y，z）映射到圖像的RGB三通道，以RGB圖像進行展示，各數據集下回歸的結果如圖7所示。

圖7 各場景下場景回歸的結果Fig.7 Result of scene regression

由場景回歸的結果可以看出，采用神經網絡對圖像的三維坐標進行回歸結果邊緣信息丟失較為嚴重，這是由于本文為了減少后續位姿求解過程的計算量，將圖像劃分成像素區域，對每一個像素區域的三維坐標真實值進行回歸，即是原始圖像三維坐標下采樣后的結果，因此丟失了大部分邊緣信息；而對于其余部分，在本文實驗的大部分場景下，圖像的三維坐標預測值和三維坐標真實值基本一致。

采用卷積神經網絡對圖像中的像素點進行三維坐標回歸也可能存在一定錯誤，因此在回歸得到查詢圖像場景坐標的基礎上，本文采用隨機抽樣的方式，隨機抽取回歸結果中一定數量的場景坐標預測值作為PnP求解過程的輸入，解得一組相機的位姿假設hi。重復以上步驟構造n組位姿假設集合（本文實驗過程中n=64），通過第三節中的RANSAC算法思路對位姿假設進行篩選，同時采用位姿對齊對估計位姿進行優化，得到最終的相機位姿估計結果。

5 位姿估計實驗結果

5.1 實驗環境

本文在公開數據集7scenes［14］以及自建數據集Labcore下對網絡參數進行訓練，其中7scenes數據集涵蓋了7種不同的室內場景，是用于相機位姿估計的一個標準數據集。實驗選取的硬件工作平臺為臺式計算機，配有NVIDIA3090顯卡一張，網絡模型通過PyTorch平臺搭建。

5.2 位姿估計精度定量分析

相機位姿估計結果的精度可用位姿估計誤差低于誤差閾值（同時滿足平移誤差閾值和旋轉誤差閾值）幀數所占總幀數的百分比以及位姿估計誤差中值來進行評價［15-16，19-20］。首先利用隨 著誤差閾值增大時，位姿估計誤差低于誤差閾值數所占測試集總數的百分比來衡量相機位姿估計的準確性。圖8為本文所提方法在自建數據集Labcore以及公開數據集7scenes上實驗的相機位姿估計準確率結果。從圖中可以看出，采用多階段相機位姿估計方法的過程中，對于大部分場景，使用SIFT算子以及SURF算子作為特征點提取時位姿估計準確率較高，在位姿估計誤差閾值為5 cm/5°的范圍內，自建數據集Labcore上的估計精度最大為87.3%，公開數據集7scenes上，數據集中環境變化較大時其表現較差（后3個場景）；同時，將稠密場景回歸網絡引入多階段相機位姿估計方法，以此構建更為稠密的2D-3D匹配點之后，不論在自建數據集還是在公開數據集上相機位姿估計的精度均有大幅度提升。

圖8 位姿估計精度隨誤差閾值變化Fig.8 Accuracy of pose estimation varies with the error threshold

圖9為引入稠密場景回歸后多階段相機位姿估計方法與其他方法在公開數據集以及自建數據集上的位姿估計準確率對比（誤差閾值范圍為5 cm/5°，7Scenes下為7個場景的平均準確率，Labcore下為單個場景準確率）?？梢钥吹皆诠_數據集下，本文方法的位姿估計準確率為82.7%，優于DSM［9］，DSAC［12］，DSAC++［15］，SANet［20］，Pixloc［16］，同時本文將現存幾種位姿估計較為優越的方法在自建數據集上進行估計，結果表明本文方法能達到94.8%的位姿估計準確率，較其余方法更加優越。其中DSAC++在自建數據集上表現相對較差，這是由于自建數據集環境變化較大，該網絡無法完全擬合數據集中的場景。

圖9 位姿估計準確率對比（5 cm/5°）Fig.9 Pose estimation accuracy comparison（5 cm/5°）

其次用平移誤差和旋轉誤差中值來衡量相機位姿估計算法的精度。圖10是本文方法在自建數據集Labcore以及7scenes數據集上進行位姿估計后的估計誤差中值結果。從圖中可以看出，對于大部分場景，本文方法的位姿估計誤差中值小于5 cm/5°。為了驗證本文提出方法的精度，根據平移誤差和旋轉誤差中值來與其他相機位姿估計方法相比較，表2為本文方法以及其他相機位姿估計方法的相機位姿估計精度，表格中的數據均來自于原始論文，加粗部分為取得各對比算法中最優的結果。從表2可以看出，采用相機位姿估計的誤差中值作為評價標準時，相較于其余相機位姿估計方法，本文方法在大部分場景中位姿估計精度均有所提升。

圖10 各場景下的中值誤差結果Fig.10 Results of median localization Errors

表2 相機位姿估計算法性能對比Tab.2 Performance comparison of camera pose estimation algorithms

5.3 位姿估計時耗定量分析

為對本文方法進行位姿估計的實時性進行評估，本文實驗中在7scenes數據集上17 000張用于測試的場景圖像進行位姿估計，并取位姿估計耗時均值作為評價指標。表3列出了本文方法與DSM［9］，DSAC++［15］，Pixloc［16］對單幀場景圖像進行位姿估計時每個步驟的耗時均值，實驗環境均為5.1節中所述。

表3 相機位姿估計算法時耗對比Tab.3 Time consumption comparison of camera pose estimation algorithms （s）

6 結論

本文通過單目相機采集的RGB數據及先驗的三維點云信息，實現相機位姿估計。首先根據深度圖像及SFM算法，構建稠密的相機位姿估計數據集。然后將深度檢索算法與基于結構的算法相結合，提出多階段相機位姿估計方法。最后，采用全卷積神經網絡回歸圖像的稠密場景信息，提出基于稠密場景回歸的多階段相機位姿估計算法。

實驗結果證明：

1.在自建數據集Labcore下，多階段相機位姿估計方法位姿估計中值誤差為平移誤差1.5 cm、旋轉誤差0.70°，且位姿估計準確率為87.3%（誤差閾值為5 cm/5°時）；基于稠密場景回歸的多階段相機位姿估計算法位姿估計中值誤差為平移誤差0.3 cm、旋轉誤差0.60°，位姿估計準確率為94.8%（誤差閾值為5 cm/5°時）。

2.在公開數據集7Scenes下，多階段相機位姿估計的中值誤差受場景變化影響較大，引入稠密場景回歸后，位姿估計的精度大幅度提高，7個公開場景下的中值誤差均小于5 cm/5°，同時平均位姿估計準確率為82.7%（誤差閾值為5 cm/5°時）。相比于目前的相機位姿估計算法，本文算法在位姿估計環節耗時更短；在位姿估計精度上，不論從位姿估計準確率還是中值誤差角度進行評價，本文所提算法均有顯著的性能提升。由于本文在圖像檢索以及場景回歸階段均采用了卷積神經網絡模型，因此在位姿估計的總時耗上相對較長。后續工作將進一步考慮實際應用的具體需求，通過簡化網絡模型的復雜度，提升本文位姿估計方法的效率。

從表3可以看出，本文在單幀場景圖像位姿估計的計算階段耗時為0.18 s，相比于其他3種方法來說耗時較短；同時，相比其余存在圖像檢索的相機位姿估計算法，本文方法相機位姿估計的總耗時最短。然而相比于文獻［15］中的DSAC++，由于本文增加了圖像檢索階段，因此相機位姿估計總耗時更長。本文方法耗時主要源于在圖像檢索以及位姿估計階段均使用了卷積神經網絡，復雜的網絡結構造成了大量耗時。因此在后續工作中，綜合實際應用場景的精度及時耗需求，通過簡化圖像檢索以及場景回歸階段的網絡模型，可提升本文位姿估計方法的效率。