基于特征融合的6D目標位姿估計算法

馬 天，蒙 鑫，牟 琦,2+，李占利，何志強

(1.西安科技大學 計算機科學與技術學院，陜西 西安 710054； 2.西安科技大學 機械工程學院，陜西 西安 710054)

0 引 言

6D目標位姿估計是指對于已知類別的目標，獲取目標到相機坐標系的單應性變換，通常使用旋轉R和平移t組成的齊次變換矩陣表示該單映性變換[1]，被廣泛應用于機器人抓取[2,3]與增強現實[4]等領域。

基于深度學習的位姿估計方法根據輸入類型劃分，分為基于RGB圖像和基于RGB-D圖像的方法。Mahdi等[5]和Tekin等[6]通過神經網絡在RGB圖像中確定目標3D邊框頂點的2D坐標，形成2D-3D對應點對，并使用PnP算法計算目標位姿。該方法對于弱紋理目標有較好的效果，但是難以處理對稱目標，于是Xiang等[7]提出端到端的網絡PoseCNN解決了該問題。基于RGB圖像的方法能夠以較快的速度完成位姿估計，并且能在一定程度上解決弱紋理目標位姿估計問題，但是易受光照和遮擋影響[8]。RGB-D圖像能夠在RGB信息的基礎上，提供額外的Depth信息進行補充[9]，從而獲得更高的位姿估計精度。Wadim等[10]使用RGB圖像獲取目標初始位姿；然后結合Depth圖像，采用ICP算法進行優化。Li等[11]提出密集融合網絡DenseFusion，該網絡將Depth圖像中的分割目標轉化為點云，使用PointNet[12]提取點云特征作為特征融合的幾何輸入，然后逐像素的將RGB圖像中目標的顏色特征與幾何特征進行融合，輸出初始位姿。Hu等[13]借助DenseFusion網絡獲得目標融合特征，然后通過Dual-Stream網絡學習不同圖像中目標特征之間的一致性，從而獲得位姿估計結果。Li等和Hu等的方法在特征融合時，單獨的對每個點進行操作，較少考慮點云之間的聯系[14]，導致幾何特征提取不足。

本文的位姿估計算法在特征融合時，通過局部區域建立點云之間的聯系，從細粒度的局部幾何特征擴展到全局，并實現幾何特征與顏色特征的融合，完成位姿估計。

1 算法思想

高精度位姿估計需要充分、準確的提取目標特征。RGB圖像和Depth圖像分別含有目標的顏色特征與幾何特征。通常一定大小的局部區域內的目標點云，共同涵蓋著目標的細節結構信息，對局部區域的點云進行特征提取，能夠獲取點云之間存在的細粒度幾何特征。然而，不同區域點云密度不一定均勻，稠密區域提取精細幾何特征的方式，可能會破壞稀疏區域的局部結構。因此，本文通過PSA(point set abstraction)[15]模塊將點云劃分為不同的局部區域，在局部區域內對不同半徑尺度空間內的點云提取特征；當局部區域的密度不同時，對容量相同，分布相近的點云提取特征，從而增強提取局部幾何特征的魯棒性，獲得較為精細的點云局部幾何特征。然后通過一個多層感知機(multilayer perceptron，MLP)在局部幾何特征的基礎上提取全局幾何特征。這樣通過PSA模塊，將點云劃分為不同的局部區域，從小區域中獲得精細的局部幾何特征，然后進一步獲取更大區域的局部幾何特征，最后通過MLP獲得點云的全局幾何特征，為位姿估計提供充足的目標幾何特征。

2 算法步驟

本文的位姿估計算法分為3個階段，如圖1所示。第一階段：目標分割。對RGB圖像進行語義分割，得到目標掩碼及最小包圍框；利用目標掩碼在Depth圖像中分割目標，并將其轉化為點云；利用最小包圍框在RGB圖像中分割目標。第二階段：特征提取與特征融合。首先在RGB圖像和點云中，分別提取目標的顏色特征和點云特征。然后選擇N個位置點，將N個位置點的點云特征與顏色特征進行融合，形成點融合特征；使用PSA模塊將點云劃分為不同局部區域，并提取局部幾何特征，與顏色特征融合形成局部融合特征；使用一個三層MLP，對PSA模塊提取局部幾何特征后的點云，提取全局幾何特征；最后將點融合特征、局部融合特征和全局幾何特征進行融合，形成最終的目標融合特征。第三階段：位姿估計與迭代優化。首先使用融合后的特征訓練網絡，輸出初始位姿；然后對初始位姿進行迭代優化，獲得更加精確的位姿估計結果。

圖1 本文6D位姿估計算法

2.1 目標分割

在目標分割階段，對RGB-D圖像中的目標進行分割。首先使用PoseCNN[7]網絡的語義分割結構對RGB圖像進行分割，得到目標掩碼及最小包圍框；然后使用目標掩碼在Depth圖像中分割目標，并通過相機內參將其轉化為點云，同時，使用最小包圍框在RGB圖像中分割目標。Depth圖像像素坐標 (u,v) 變換為點云 (X,Y,Z) 公式如式(1)

Z=z(u,v)

(1)

其中，Depth圖像中每一個像素值z(u,v) 表示現實場景中的點到相機鏡頭的距離。fu和fv分別代表水平焦距和垂直焦距， (u0,v0) 為相機中心坐標。

2.2 特征提取與特征融合

在特征提取與特征融合階段，獲取目標的顏色特征與點云特征，并將兩者進行融合。特征提取在RGB圖像和點云中，分別提取目標的顏色特征和點云特征，作為特征融合的輸入。與DenseFusion[11]算法相同，本文使用Resnet18網絡進行編碼，提取目標的顏色特征，通過4個上采樣層進行解碼，同時，使用PointNet[12]網絡提取點云特征。

特征融合在點云中選擇N個位置的點云，并借助相機內參在圖像中找到對應投影位置的像素點，對N個位置點的特征進行融合，這樣對目標可見部分的特征進行融合，可以降低目標遮擋以及目標分割可能存在偏差的影響。首先將N個位置點的顏色特征與點云特征進行融合，形成點融合特征；然后使用PSA模塊在局部區域內對點云提取局部幾何特征，與顏色特征融合后形成局部融合特征。PSA模塊提取局部幾何特征過程如下：①使用最遠點采樣的方式，找到特定數量且在空間中均勻分布的點云。②分別以這些點云所在位置為中心點，將固定半徑的球體劃分為一個局部區域。③使用PointNet網絡在局部區域內，分別對不同半徑尺度球體內的點云提取特征，并將特征進行連接聚集。④用整個局部區域的特征表示位于中心點位置點云的特征。以二維舉例，PSA模塊如圖2所示。最后對PSA模塊提取局部幾何特征后的點云，使用一個三層MLP提取全局幾何特征，將點融合特征、局部融合特征和全局幾何特征進行融合，形成最終的目標融合特征。

圖2 二維PSA模塊

2.3 位姿估計與迭代優化

2.3.1 損失函數

損失函數通過置信度對位姿損失進行加權，并借助對數函數將置信度作為正則化項。位姿估計網絡整體損失函數如式(2)

(2)

(3)

(4)

其中，xk表示與xj距離最近的點。

2.3.2 迭代優化

迭代優化需要以上一步獲得的位姿為基礎，為滿足這一要求，將點云通過初始位姿變換為新點云，新點云便可對上一步的位姿進行隱式編碼；然后，將新點云輸入到網絡中，提取點云幾何特征，與顏色特征融合后預測位姿殘差，進而調整位姿結果。迭代優化部分由4個全連接層組成，經過K次迭代后，最終位姿估計結果計算公式如式(5)

(5)

其中， [RK|tK] 表示第K次迭代后的位姿估計結果。本文K設置為2。

3 實驗及分析

3.1 實驗環境

本文提出的基于特征融合的6D目標位姿估計算法，實驗環境見表1。

表1 實驗環境配置

3.2 數據集及評價指標

3.2.1 數據集介紹

本文采用的數據集為位姿估計標準數據集LineMOD以及YCB-Video。

LineMOD數據集由Hinterstoisser團隊維護，包含15種復雜背景下弱紋理目標的RGB-D圖像序列。每個序列僅對一種目標進行位姿估計，該目標被放置在帶標記平面板的中心位置，除該目標之外的另外14種目標隨意擺放，標記用于提供目標相應的Ground Truth位姿。每個序列包含1100～1300張RGB-D圖像。LineMOD數據集對13種目標進行實驗，分別為Ape、Benchvise、Camera、Can、Cat、Driller、Duck、Eggbox、Glue、Holepuncher、Iron、Lamp以及Phone。其中，Eggbox與Glue兩種目標具有旋轉對稱性，每種目標尺寸均不相同。

本文對以上13種目標序列進行實驗，將每種目標15%的RGB-D圖像劃分為訓練集，其余為測試集。訓練集包含13種目標總2372張RGB-D圖像，測試集包含13種目標總13 406張RGB-D圖像。

YCB-Video是近年來開始使用的大型位姿估計視頻數據集，由Xiang團隊維護。數據集根據21種不同尺寸、不同紋理的YCB目標，從中任意選取3-9種目標搭建不同真實室內場景，每個場景中的目標都存在遮擋情況。對于不同場景的目標，使用RGB-D相機拍攝制成92個視頻，然后對各視頻間隔7幀提取一個關鍵幀，保存為RGB-D圖像，并且使用合成圖像擴大數據規模。YCB-Video還提供目標Ground Truth位姿和分割掩碼。

本文使用編號0000-0047、0060-0091視頻序列，以及合成的80000幀RGB-D圖像數據用于訓練，使用0048-0059視頻序列共2949幀RGB-D圖像進行測試。

3.2.2 評價指標

(6)

其中，M表示目標3D模型采樣點集合，x表示采樣點集合的第x點，m表示采樣點數量。ADD精度為：正確位姿估計的數量占全部Ground Truth位姿數量的百分比；如果ADD小于閾值，則認為位姿估計正確。ADD精度計算公式如式(7)

(7)

其中，Numpre表示正確位姿估計的數量，NumGT表示全部真實位姿的數量。

(8)

其中，M表示目標3D模型采樣點集合，x1，x2表示屬于不同變換的采樣點，x2為與x1距離最近的點。

ADD-S精度為：正確位姿估計的數量占全部Ground Truth位姿數量的百分比；如果ADD-S小于閾值，則認為位姿估計正確。ADD-S精度計算公式與式(7)相同。

(3)AUC面積。使用ADD-S指標計算不同閾值(最大閾值為0.1 m)下的位姿估計精度，然后繪制閾值—精度曲線(accuracy-threshold curve)。AUC面積表示閾值-精度曲線與閾值坐標軸圍成的面積。

3.3 實驗結果對比與分析

本文實驗，對于LineMOD數據集，使用ADD精度評價非對稱目標，使用ADD-S精度評價對稱目標，閾值均設置為目標最大直徑值的10%。對于YCB-Video數據集，使用ADD-S精度(閾值設定為0.02 m)與AUC面積評價全部目標。

圖3為本文位姿估計算法在LineMOD數據集和YCB-Video數據集的可視化結果。LineMOD數據集只對位于標記板中心位置的目標進行位姿估計，紅色點為當前目標3D模型上的采樣點，經過估計位姿變換，然后投影到圖像上的點，投影點與目標越契合，表示位姿估計結果越準確。YCB-Video數據集對場景中的全部目標進行位姿估計，不同目標投影點顏色不同。框選的目標表示，與對比算法相比，位姿結果差異較大的目標；從可視化結果可以直觀看出，本文算法結果較優。

圖3 位姿估計可視化結果

表2為LineMOD數據集，本文算法分別與其它算法未優化和優化的精度對比。表2第一行為目標實例名稱，名稱帶有“*”號標記表示目標具有旋轉對稱性，最后一列為13種目標的平均位姿估計精度。

表2 LineMOD數據集精度結果對比/%

表中數值表示不同算法對于當前目標的位姿估計精度，數值越大說明該方法對當前目標的位姿估計效果越好。加粗表示該方法對于當前目標的位姿估計精度最高。

對于LineMOD數據集，本文算法分別與Real-Time[6]、PoseCNN[7]、DenseFusion[11]以及Dual-Stream[13]算法進行對比。Real-Time算法不需要對位姿估計結果進行優化，PoseCNN算法未優化時精度較低。因此，此處只與Real-Time算法未優化，PoseCNN算法優化結果進行對比。其中，PoseCNN使用ICP算法進行優化，Dual-Stream與本文均使用DenseFusion提出的迭代優化算法進行優化。

實驗結果顯示，未對位姿估計結果優化時，本文算法13種目標的平均精度與Real-Time算法平均精度相比提高31.5%，與DenseFusion相比提高1.4%，與Dual-Stream相比提高0.5%。位姿估計結果進行優化之后，對于13種目標的平均精度，本文算法優于其它算法；與本文算法未優化相比，平均精度提高7.7%。本文算法未對位姿估計結果進行優化時，對Ape、Duck和Holepuncher這3種目標位姿估計精度相對較低，原因可能有兩種，其一，本文實驗使用點集抽象提取局部幾何特征時，13種目標選擇相同的多尺度半徑，導致上述3種目標提取的局部幾何特征與其它目標相比不夠精細；其二，本文算法位姿估計網絡損失兼顧全部目標，而驗證時要求ADD小于目標最大直徑的10%則認為位姿估計正確，以上3種目標的最大直徑較小，均低于0.14 m，Ape目標最大直徑僅有0.1 m，評價閾值相對較低，導致精度降低。除小型目標之外，本文算法對其它目標的精度均有較優的表現，說明本文算法充分提取目標幾何特征，并與顏色特征進行融合的方法，能夠提升弱紋理目標的位姿估計精度，對于弱紋理目標具有較優的表現。

表3為YCB-Video數據集，本文算法與其它算法未優化時AUC面積與精度的對比結果。表3第一列為目標實例名稱，名稱帶有“*”號標記表示目標具有旋轉對稱性，最后一行為21種目標評價指標的平均值。表3數值表示每種方法AUC面積與ADD-S精度兩個評價指標的結果，數值越大說明該方法對當前目標的位姿估計效果越好。加粗表示該方法對于當前目標的評價指標結果最高。

由表3可知，本文算法21種目標的AUC面積平均值與PoseCNN算法的AUC面積平均值相比提高5.6%，與DenseFusion相比提高1.0%，與Dual-Stream相比提高0.5%。本文算法ADD-S精度平均值與PoseCNN相比提高16.3%，與DenseFusion相比提高0.9%，與Dual-Stream相比提高0.4%。本文算法對于051_large_clamp和052_extra_clamp目標表現與其它目標相比較差。這是因為，兩個目標差別較為細微，網絡難以對兩者進行區分，導致位姿估計表現與其它目標相比較差。除上述兩種目標，本文算法對于其它目標均有較優的表現，說明本文算法充分提取目標幾何特征，并與顏色特征進行融合，能夠提升遮擋目標的位姿估計精度，對于遮擋目標具有較優的表現。

表3 YCB-Video數據集AUC面積及ADD-S精度結果對比/%

表4為Real-Time、PoseCNN和DenseFusion這3種算法與本文算法位姿估計所用總時間的對比結果。主要分為，分割時間、位姿估計時間以及優化時間進行對比。

表4 Real-Time、PoseCNN、DenseFusion和本文算法運行時間對比/s

Dual-Stream算法原文未提及算法用時，且論文代碼尚未開源，此處不與其作對比。Real-Time算法不需要進行分割，分割時間為零。PoseCNN、Densefusion與本文算法均使用相同的語義分割方法，因此，分割時間相同。不同算法使用不同優化策略進行優化，優化用時也不相同。其中，Real-Time算法不需要對位姿估計結果進行優化；PoseCNN使用ICP算法進行優化，耗時最長；本文使用DenseFusion算法的迭代優化方法，優化時間為0.01 s。由表4可知，本文算法完成一次位姿估計總時間與最快的Real-Time算法相差0.11 s，時間表現中等。這是因為，使用點集抽象提取局部幾何特征，需要在3D空間確定局部區域中心點位置，耗費一定時間。

4 結束語

本文算法在局部區域內獲取點云精細的幾何特征，并與顏色特征進行融合，實現位姿估計。實驗結果表明，該算法對于LineMOD數據集中體積中等或較大的目標，位姿估計精度有明顯提升，對于LineMOD數據集與YCB-Video數據集所有目標的平均位姿估計精度均有較優的表現。但是，由于提取局部幾何特征需要耗費一定時間確定區域中心位置，算法時間表現中等。

續上表由于提出的算法是一種先進行特征提取，然后進行特征融合的方法。所以，在特征提取過程中，兩個分支沒有交互，限制了提取的目標特征的表征能力。因此，在未來工作中，考慮在特征提取的過程中進行特征融合，并且從獲取點云局部幾何特征的角度降低算法耗時。