基于SfM單目六自由度位姿估計數據集自動標注

劉 毅，魏東辰，李子豪，嚴小軍

(1.北京航天控制儀器研究所 工藝技術研究室，北京 100854；2.北京易航遠智科技有限公司 感知部，北京 100015；3.北京航天控制儀器研究所 測試技術研究室，北京 100854)

0 引 言

物體六自由度位姿估計技術主要提供物體在某個參考系中定位定姿的功能。單目相機下物體的六自由度位姿一般指的是物體在相機坐標系中的平移(x，y，z)與物體相對于相機坐標系的旋轉(roll，yaw，pitch)。隨著深度學習與機器視覺的融合交叉，傳統物體六自由度位姿估計方法有被基于深度學習的方法取代的趨勢[1]。深度學習算法作為以數據為驅動的方法，高質量訓練數據對于訓練結果有著積極作用[2]。

目前業界比較常見的六自由度估計數據集[3]有LineMOD、Occlusion、T-LESS、YCB-Video等，其中以LineMOD數據集形式為主，包含15種物體，每一種物體有1200張圖像左右，數據形式分別為：原始圖像、物體三維模型及尺寸信息、物體位姿可視化圖像、物體掩碼mask圖像、物體相對于相機的旋轉矩陣文本信息和圖像對應相機內參。數據集的標注并不是一件容易的事，由于二維圖像缺乏深度信息，造成6個自由度中深度、旋轉量的人工標注差異性大，精度不高。LineMOD數據集制作借助RGB-D相機，使用基于三維模型模板的方法，在空間中離散選取視點，通過物體顏色梯度和表面法線特征進行匹配求解位姿，但深度信息中法線不穩定，需要復雜的算法保證。物體六自由度位姿標注方法常常基于深度相機或者雙目相機[4]，在工業界或高校等硬件受限的場景中，這些帶有特殊傳感器或者需要精確安裝的成像方式并不容易實現。另外也有通過渲染，將三維模型按照特定的位姿渲染到任意圖像中制作合成數據集[3，5]，這樣的方法忽略真實場景中的光影信息，缺乏真實感，而且無法生成具有遮擋現象的數據集。其次，三維模型往往顏色并不與真實相符，使得合成的數據集僅有幾何信息被正確保留。

1 數據集制作流程及SfM算法分析

1.1 數據集制作流程

本文中，指定采集數據集的工作場景為室內小場景。數據集制作流程如圖1所示，位姿關系解算過程如圖2所示。首先根據CAD模型加工實物，同時采用均勻下采樣算法處理物體CAD模型表面，得到模型點云；使用單目RGB相機在實驗場景中獲取物體多視角圖像集；然后使用SfM[6]算法獲取拍攝場景恢復尺度的點云信息，并獲取每張圖像的相機參數；將已獲得的模型點云并放置在SfM獲取的點云空間中，使用ICP(iterative closet point)算法獲得物體點云相對于世界坐標系的位姿關系；最后，通過坐標變換獲得每一張圖像中物體相對于相機的位姿，從而獲得物體六自由度位姿估計訓練所需的數據集。

圖1 數據集制作流程

圖2 位姿解算過程

制作六自由度位姿估計數據集的輸入是二維圖像，但由于二維圖像缺失深度信息，通過人工標注的方法并不可靠。SfM可以通過對輸入圖像集的處理，得到場景點云模型和圖像集中每一張圖像對應的相機參數，相機參數中包含了相機相對于點云空間坐標系的位姿。SfM算法的性能對于位姿解算結果的精度具有重要影響。

1.2 SfM

SfM算法目前主要分為3種形式：增量式運動恢復結構(incremental structure from motion，Incremental SfM)、全局式運動恢復結構(global structure from motion，Global SfM)、分層式運動恢復結構[7](hierarchical structure from motion，Hierarchical SfM)。在一般情況下，數據采集過程通過采用同一相機進行連續拍攝得到連續的圖像幀完成。另外，增量式運動恢復結構在重建過程中不斷使用捆綁調整進行優化場景結構，所以選擇精度更高的增量式運動恢復結構更為合適。增量式運動恢復結構算法在每輸入一定數量圖像后會進行一次捆綁調整(bundle adjustment，BA)優化場景結構，該方法已經在各主流三維重建平臺使用，例如VisualSfM、Bundle、Colmap等。

增量式運動恢復結構流程如圖3所示，首先利用特征提取、特征匹配和幾何驗證的方法，確定輸入數據集中各圖片之間的匹配關系，并將多個視角具有相同描述子的特征點進行連接；按照幾何關系準則選取初始相機對，以第一個相機坐標系的原點為坐標系原點，通過三角測量方法建立初始三維點云場景；然后輸入新的圖像，進行增量式重建，對于后續加入的圖像利用多點透視方法(perspective-n-point，PnP)求解圖像對應相機的外參，利用三角測量確定點云信息，并根據已加入場景的視圖，決定是否將當前場景不存在的新點云添加到場景信息中。在增量式重建中，相機位姿求解和三角測量是兩個單獨的過程，由于算法具有不確定性，將會帶來不同的誤差，甚至相互影響。為此引入捆綁調整，通過使重投影誤差最小實現優化，從而提高點云信息和相機參數信息的精度。

圖3 SfM流程

增量式運動恢復結構最后的輸出是場景點云模型和圖像對應的相機參數，點云模型通過多張圖像中匹配的特征點三角測量得到，相機參數通過相機標定與攝影幾何求得，在這個過程中由于錯誤匹配、優化算法的不穩定性等使得輸出結果并不理想。為了提高求解的穩定性，可以通過兩方面解決：約束的正確性和約束的充分性。約束的正確性表現在特征提取與匹配的高效正確性，SfM算法的假設條件是物體表面為純漫反射，而在實際數據集采集過程中，無法確保物體表面的材質，當物體表面為鏡面反射時，光線照射在鏡面反射區域會直接反射到相機中，導致出現高光區域，丟失了物體原本的表面紋理信息，造成特征提取不準確；對于約束的充分性，可以通過先驗約束、多傳感器融合等方法提高求解的穩定性，本文將通過具有先驗信息的標定板來提高約束的充分性，實現尺度的恢復以及更加準確的位姿估計。

2 數據集自動標注方法

SfM算法首先利用SIFT算法獲取圖像集每一張圖像的特征點和對應的描述子，對圖像集中不同圖片的描述子使用L2范數進行特征匹配，構建匹配圖像對，同時設置圖像對中第一幀圖像的相機坐標系原點作為原點建立點云坐標系和初始化點云空間。通過增量輸入圖像的匹配以及使用PnP算法解算，獲得不同圖像在該點云坐標系下的相機外參。對于新的特征點，使用三角測量進一步擴充點云空間，此時，便可獲得拍攝數據集的點云場景模型以及每張圖像對相應的相機內外參數。由于三維點云由不具備深度信息的二維圖像恢復而成，在不采用任何額外處理時，SfM生成的三維點云存在尺度不準確的問題。此外，相機外參僅能反應三維點云所在坐標系相對于相機的位姿變換，而不是目標物體相對于相機的位姿變換，因此需要在點云中對目標物體的位姿信息進行提取，得到目標物體相對于點云坐標系的坐標變換。為了提高數據集的標注精度，并與LineMOD、YCBVideo的數據標注格式保持一致，需要對SfM算法進行改進，并對結果做進一步處理。

2.1 特征提取與特征匹配

在三維重建過程中，由于場景中存在高光現象、動態物體等，使得圖像特征提取不準確，并產生錯誤的特征匹配，不利于后續三角測量、BA優化等過程，所以需要額外的措施來增加算法的魯棒性。對于工業室內可控場景，可以避免動態物體的存在，但由于光照影響，高光邊緣處會產生錯誤的特征匹配和錯誤的顏色信息，使得重建結果周圍存在白色噪點。

為了獲得計算復雜度低、泛化能力強、魯棒性好的高光檢測閾值算法，采用無高光(Specular-free，SF)模型[8]對朗伯體金屬表面建立表面反射模型進行高光區域檢測。

假設RGB相機像素值與光強成正比，物體表面光反射由漫反射與鏡面反射線性組合而成，將物體表面在RGB圖像中的色彩分解成漫反射和鏡面反射兩部分，對于每一個獨立反射部分，都可分解為光譜分量與幾何分量，利用圖像RGB三通道值信息，建立SF模型

Vi(p)=α(p)Vb，i+β(p)Vs，i

(1)

VSF，i(p)=Vi(p)-Vmin(p)=α(p)(Vb，i-Vb，min)

(2)

其中，Vi(p) 表示像素p的i通道響應值；α(p)、β(p) 分別是漫反射因子、鏡面反射因子；Vb，i(p)、Vs，i(p) 分別是材料本身顏色與光源顏色；這里假設光源為白色光板，即響應值為255常量，那么得到VSF，i(p) 不再包含鏡像反射因子。此時在SF圖像與原始圖像漫反射分量SF圖像的p像素處，獲得的相差一個偏差值τ(p)=Vmin(p)-β(p)Vs，i。 為了使閾值在圖像中具有泛化能力，設定閾值為

(3)

其中，μv和σv是所有像素Vmin(p)的均值和方差，η與背景圖像對比度、圖像的反射程度相關。

SFM算法中經常采用尺度不變特征(scale-invariant feature transform，SIFT)，該特征提取方法是目前最復雜的特征檢測和描述方法之一，具有很強的探索性，例如colmap采用的DSP-SIFT。采用SIFT特征點檢測的圖像輸入是單張圖像灰度圖，然后進行下采樣得到尺度縮放后的若干灰度圖構成尺度空間。由于在圖像尺度縮放過程中，高光區域的像素值發生變化，所以對不同尺度空間的圖像進行高光區域檢測。

采樣后的灰度圖通過不同標準差的高斯核函數對其進行采樣獲得圖像金字塔，并在每個尺度空間的相鄰圖像層做差得到高斯差圖像，最后利用相鄰高斯差圖像進行極值求解獲得初始關鍵點。對于單張獲得的初始關鍵點，將特征點中高斯函數差值具有低對比度特征點進行篩除。由于高亮處像素處對比度較高，所以很容易被識別為特征點并保留。

對于圖像中的高光區域，a鏡面反射使得物體表面的顏色信息、紋理信息丟失，甚至改變物體表面信息。對于圖像信息丟失處，Criminisi算法可以利用周圍近似像素進行近似補償，但這樣產生的特征信息并不具有參考意義，甚至導致錯誤匹配，所以對于高光區域檢測到的特征采取直接過濾處理。

2.2 后端處理

通過對圖像進行特征提取，獲得SIFT特征點進行圖像匹配，將具有相同描述子的特征點歸納到統一的集合中。為了獲得場景的點云模型，與每一張圖像對應相機的參數，需要對前端獲得的特征信息進行后端處理。

在六自由度位姿估計數據集中，為了使用物體位姿信息將三維標定框正確錨住物體，則必須知道物體的原尺度以及在圖像中的尺度信息，對三維重建點云空間恢復尺度[9]。對于小場景的尺度恢復，采用在場景中布置先驗尺度信息標定板的方法，首先對圖像中出現的特定標志點提取像點信息，利用三角測量得到像點對應的物點信息，然后使用先驗的尺度信息來標定物點之間的距離

xi×TiX=0i=1，2，…，n

(4)

(5)

(6)

式中：T為相機外參組成的旋轉矩陣；i為參與三角測量的圖像編號；j為標定板上標志點對應的三維點；c為標志點之間的先驗尺度信息。

在增量重建過程中，添加的圖像會進行PnP和隨機抽樣一致性處理，將得到的新特征信息進行匹配與重建。為了使得位姿數據精度提高，則在BA過程加入先驗物點與像點對應關系的約束，如式(7)所示

(7)

在SfM算法中，除特殊規定外，世界坐標系的原點與選定初始化點云的第一幀圖像相機光心重合，得到的世界坐標系位置過于隨機。為了方便后面對點云進行旋轉、移動，利用sim(3)變換將在初始化點云過程中將世界坐標系轉化到標定板特定位置處，并在之后添加圖像進行增量重建過程中，基于此坐標系進行三角測量與BA優化。

2.3 點云配準

經過三維重建之后，可以得到數據集采集場景的點云信息和每一張圖像對應的相機參數。然而SfM得到求解信息為每個圖像的相機內外參數和點云空間模型，無法直接獲得目標物體點云在點云空間中的位姿信息。為了獲得該信息，需要將物體點云與三維模型下采樣得到的模型點云進行配準。

目前已經獲得固定坐標系的點云空間，根據目標物體CAD模型的尺寸信息，可以很容易地設定PCL的PassThrough濾波器數值，將xyz軸固定距離范圍之外的點直接過濾，為了降低噪點，根據點云歐式距離計算將鄰近點數目較少的點過濾，從而將目標物體在點云空間中分割出來。將預先準備的三維模型均勻下采樣得到模型點云，導入到處理后的點云空間坐標系中，使得模型點云的中心與點云空間坐標系原點重合，其標定姿態的xyz軸與點云空間坐標系xyz軸平行。利用ICP點云配準算法將三維重建的場景模型配準到三維點云模型中，獲得三維重建模型與世界坐標系之間的旋轉矩陣。

小場景下點云數量較少，使用ICP點云配準算法[10]進行三維重建點云與三維模型點云之間的配準。其算法思想與SfM類似，首先對點云進行特征點提取與特征描述，然后找到相似特征確定數據的重疊部分。得到一組配準后的3D點后，利用以下等式估算變換矩陣

X={x1，x2，…，xn}，X′={x′1，x′2，…，x′n}

(8)

?i∈[1，n]，xi=Tx′i

(9)

(10)

對于點云提取特征，ICP算法解決的是一個高維的非線性優化問題，容易陷入局部最優解，所以需要讓配準的兩個點云盡可能過濾噪點，并具有良好初始位置關系。

2.4 坐標變換

對于物體位姿，一般使用SE(3)來表示，對于目標物體在相機坐標系中的位置使用 {x，y，z} 表示，而對于目標物體的旋轉表達有兩種形式，第一種是物體相對于自身坐標系的旋轉，第二種是物體相對于相機坐標系的旋轉。前者經常使用在自動駕駛3D目標檢測，后者用在機械臂抓取、增強現實等領域。另外，深度學習參數量龐大，為減小不必要的運算，會采用感興趣區域處理機制。在對物體位姿估計之前采用平面檢測，提取物體二維標定框區域，這就導致物體以相同輪廓出現在圖像不同位置時，對應的旋轉量不同，這對于數據驅動的深度學習算法是不能接受的，所以使用目標物體相對于物體自身坐標系的表達方式更適合基于檢測后的深度學習算法[11]；而基于全圖信息的深度學習算法，可以直接采用物體相對于相機坐標系的旋轉表達。在六自由度位姿估計中，最后獲得的數據應是目標物體相對于相機的位姿，所以第一種表達方式需要在神經網絡解算之后，添加攝影幾何進行求解最終位姿[12]。

為了獲得物體的三維標定框，需要計算物體相對于相機的位姿。目前，通過點云配準獲得物體相對于世界坐標系的旋轉矩陣、通過SfM獲得每一張圖像相機相對于世界坐標系的旋轉矩陣，那么通過坐標變換就很容易得到物體相對于每一張圖像相機的旋轉矩陣，即物體相對于相機的位姿

(11)

對于物體的三維標定框，可以在模型點云初始位置處，確定點云匹配前模型三維標定框的8個頂點；然后利用Rco和tco將頂點在圖像中的位置求出，按順序連接便得到可視化三維標定框。

3 實驗驗證與分析

3.1 數據集制作及分析

在正常光照的室內場景中對奶盒進行拍攝，采用Realsense D455相機單目模式，采集圖像大小為1280*800像素，相機參數為自動，選擇實驗數據集對象為奶盒；采用大小為5×4、棋盤格邊長為50 mm的ChArUco圖案作為棋盤格標定板，以在恢復三維點云尺度信息時作為先驗信息；數據采集過程中相機與物體距離保持在0.5 m左右，進行環繞拍攝，分別制作無遮擋、有遮擋的數據集。在有遮擋的數據集中的實例級語義圖，需使用Label Studio等工具手工標注，位姿數據仍可以快速自動獲得。

算法改進基于colmap框架，設置相機模型為已知相機內參模型，輸入拍攝的圖像集進行三維重建；利用本文提出改進方法前后對比如圖4所示，經過改進后所得到的點云噪點減少，同時點云空間坐標系固定在標定板上；將物體三維模型下采樣為點云模型，由于點云空間坐標系與物體點云具有較好的初始位置關系，則可以通過手動調節為ICP算法提供一個優秀的初始值；使用ICP算法，便獲得物體點云在點云空間中的精確位姿。已知每一張圖像在點云空間的相機外參和物體相對于空間的位姿，利用坐標變換就得到物體相對于相機的位姿關系。

圖4 改進前后的點云模型

Blender是一款開源的三維圖形圖像軟件，提供建模、渲染等功能，支持python控制臺輸入與腳本運行，結合后臺終端可以激活python虛擬環境，從而使用更多的擴展包。為了檢驗位姿的正確性和獲得mask數據，使用Blen-der[13]軟件的python控制端口，利用Blender中的bpy庫Opencv仿射變換將模型渲染到圖像上。將圖像集和圖像對應的相機參數導入Blender中，設定內置相機參數，可以獲得圖像拍攝虛擬場景。由于不同的數據集或者計算機視覺算法庫對坐標系的定義不同，所以需要注意blender空間坐標系以及位姿參數設置與SfM得到結果之間的轉換關系。三維模型導入放置在世界坐標系原點處，將先前ICP得到的旋轉矩陣轉化成Blender參數的表示方式并輸入，使三維模型在點云空間中與物體點云重合如圖5所示。使用之前求得的相機模型將模型點云渲染到空白圖片中，通過設定像素閾值獲得mask文件，利用python控制臺擴展獲得其它文件。

圖5 blender渲染

利用本文提出方法獲得的無遮擋可視化數據集如圖6所示、有遮擋可視化數據集如圖7所示。

圖6 數據集可視化，從上往下分別為原圖、三維標定框、mask

圖7 遮擋數據集可視化，從上往下分別為原圖、三維標定框、mask

3.2 訓練模型與評估標準

本文采用EfficientPose[14]作為檢測算法，EfficientPose屬于直接回歸六自由度位姿估計算法，使用平面圖像為輸入，通過神經網絡直接回歸得到物體六維姿態。EfficientPose位姿估計算法是在EfficientDet[15]二維檢測算法基礎上添加平移頭回歸網絡和旋轉頭回歸網絡，在神經網絡訓練過程中可以基于EfficientDet已訓練好的二維檢測模型進行訓練，其網絡結構如圖8所示。選擇phi=0的EfficientNet[16]神經網絡框架，訓練過程中采用tensorflow的Adam優化器，學習率設置為1e-4，訓練500個迭代周期。為加速訓練，模型訓練使用兩塊NVIDIA RTX2080Ti GPU。

圖8 神經網絡框架

重投影誤差指將目標物體的三維模型點云作為輸入，利用物體相對于相機的位姿信息將模型頂點投影到圖像平面上，真值與估計值之間誤差利用下式確定

(12)

式中：M為點云的集合，K為相機參數。由于尺寸縮放，齊次坐標在計算L2范數之前先歸一化處理。

平均距離[17](3D average distance，ADD)是指目標物體的三維模型點云作為輸入，通過位姿變換預測值與位姿變換真值分別求得變換后的點云坐標，計算所有點的平均距離，如下所示

(13)

式中：R′為估計得到的旋轉量，t′為估計得到的平移量。當ΔADD小于模型直徑的10%時，判定估計的姿態是正確的。對于對稱性物體，如果表面紋理相同，則在特定角度下會有對稱解存在，而平均距離考慮的是唯一解。為了能夠處理對稱解的情況，引入ADD(-s)準則[18]，其計算式為

(14)

3.3 結果分析

基于Colmap框架對特征提取與后端處理進行改進，改進前后得到點云的均方誤差見表1。

表1 改進前后點云重投影誤差

通過實驗結果可知，對高光處錯誤特征點處理，可以提高約束的正確性；通過添加充分的先驗約束，提高約束的精度。

與經典機器視覺基于特征點幾何關系算法有較大區別，EfficientPose網絡采用基于神經網絡的直接回歸算法，從圖像像素中學習到位姿估計的規律，這就造成位姿估計過度依賴于圖像像素信息，造成泛化能力低，當場景發生較大變化時，算法失效。本實驗背景為小場景中物體的位姿估計，場景變化不明顯。分別在有標定板、無標定板的圖像中測試利用本文訓練得到EfficientPose網絡，其結果如圖9所示。

圖9 有無標定板位姿估計對比

在相同的參數設置下，通過實驗發現，與LineMOD標準數據集相比，我們通過單目RGB相機獲得的數據集，同樣可以訓練神經網絡，并獲得較好的位姿估計效果，結果見表2；另外，數據集的大小與數據集采集時間相關，后續處理工作量與數據集采集時間無關，所以可以以更簡單的方法獲取大量數據集。

表2 神經網絡訓練在相同環境下的結果

本數據集是用于六自由度位姿估計的訓練的數據集，如表3所示與現在的數據集制作方法相比有以下區別：首先，在真實場景中采集數據，僅通過單目RGB相機，利用SfM、ICP等算法，以更簡單的方法實現對大數量的圖像集進行六自由度位姿快速標注；獲取到的數據集反映了目標在場景中真實的位姿、光影效果、表面顏色紋理、物體遮擋等信息；其次，使用具有先驗信息、特征信息魯棒性強的標定板輔助定位，在有遮擋場景、弱紋理物體、對稱物體，也可以建立數據集。

表3 真實場景中物體位姿估計數據集對比

4 結束語

本文詳細敘述了基于SfM的單目RGB六自由姿態估計數據集的創建過程、SfM的前端針對高光區域特征點的設計、后端增加先驗約束處理和數據集數據的獲得方式，最后使用數據集訓練EfficientPose來驗證方法的有效性和可行性。實驗結果表明，與現有制作方法相比，本文方法在相同有效性下，降低對設備和場景布置的要求，且針對有遮擋的情況具有很強的魯棒性。

由于場景中布置標定板，使得神經網絡學到的特征中包含標定板顯著特征，從而降低模型的泛化能力，因此未來需對不存在顯著特征的先驗信息進行研究。