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

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

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

0 引 言

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

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

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

1 算法思想

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

2 算法步驟

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

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

2.1 目標分割

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

Z=z(u,v)

(1)

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

2.2 特征提取與特征融合

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

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

圖2 二維PSA模塊

2.3 位姿估計與迭代優(yōu)化

2.3.1 損失函數(shù)

損失函數(shù)通過置信度對位姿損失進行加權(quán)，并借助對數(shù)函數(shù)將置信度作為正則化項。位姿估計網(wǎng)絡(luò)整體損失函數(shù)如式(2)

(2)

(3)

(4)

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

2.3.2 迭代優(yōu)化

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

(5)

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

3 實驗及分析

3.1 實驗環(huán)境

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

表1 實驗環(huán)境配置

3.2 數(shù)據(jù)集及評價指標

3.2.1 數(shù)據(jù)集介紹

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

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

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

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

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

3.2.2 評價指標

(6)

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

(7)

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

(8)

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

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

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

3.3 實驗結(jié)果對比與分析

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

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

圖3 位姿估計可視化結(jié)果

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

表2 LineMOD數(shù)據(jù)集精度結(jié)果對比/%

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

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

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

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

由表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目標表現(xiàn)與其它目標相比較差。這是因為，兩個目標差別較為細微，網(wǎng)絡(luò)難以對兩者進行區(qū)分，導(dǎo)致位姿估計表現(xiàn)與其它目標相比較差。除上述兩種目標，本文算法對于其它目標均有較優(yōu)的表現(xiàn)，說明本文算法充分提取目標幾何特征，并與顏色特征進行融合，能夠提升遮擋目標的位姿估計精度，對于遮擋目標具有較優(yōu)的表現(xiàn)。

表3 YCB-Video數(shù)據(jù)集AUC面積及ADD-S精度結(jié)果對比/%

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

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

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

4 結(jié)束語

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

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