一種基于實例分割和點云配準的六維位姿估計方法

侯大偉

(中國科學技術太學 信息科學技術學院，安徽 合肥230026)

0 引言

點云配準是三維重建、工業測量、機器人抓取等方面的一種常見方法，目標是將不同視角下點云拼接成一塊完整的點云數據。隨著深度相機的廣泛應用，研究人員可以便捷地獲取三維空間的點云數據，用以估計物體的六維位姿。 圖 1(a)和圖 1(b)是在不同視角下的存在交集的兩片點云，不斷地調整交集部分的點云直到基本重疊，最終兩片點云拼接為一個整體(圖 1(c))。

以上兩片點云配準的過程，本質上得到的是兩片點云之間相對位姿的變換矩陣。假設其中一片點云參考目標，即相對世界坐標系的位姿參數均已知，便可推理得到另一片點云的位姿，從而實現目標點云的位姿估計。

圖1

目前基于RGB-D 圖位姿估計的方法一般先從RGB 圖片中獲取物體初始姿態，再依據 RGB-D 圖用ICP 算法[1]迭代優化得到最終姿態，如HINTERSTOISSER S[2-4]提出的 LINEMOD 算法，它針對物體的3D 模型，從各個不同的視角來提取 RGB 圖像和 RGB-D 圖像來生成模板，再與實際的圖像去匹配，得到初始姿態估計后再用ICP 算法進行優化。CHAI Z Q 等[5]提出利用全卷積網絡(Fully Convolutional Networ-ks，FCN)分 割 RGB 圖 像 ，與 LINEMOD算法相結合的新方法，實現對圖像中目標的快速檢測和定位。 陳啟軍等[6]將 RGB-D 圖轉換成 HHA 特征圖和場景點云后進行物體的實例分割，并依據4PCS 算法和ICP 算法，將分割出的目標點云和目標CAD 模型的模型點云進行匹配和位姿精修，從而得到精確的位姿估計結果。 CHI L 等[7]通過端到端深度學習模型同時分析RGB 圖片和 RGB-D 圖片，獲取初始的姿態后輸入3D 信息來優化出最終的六維位姿。 HODAN T 等[8]使用預過濾器來處理輸入圖像，忽略置信度低的區域，并對其他位置采用匹配特征點的方法生成模板并驗證。 但是該方法只能獲得近似的對象姿態，還需要隨機優化過程來估計準確的六維姿態。

本文采用的是先分割RGB-D 圖后進行點云配準來估計六維姿態的方法。 整體的流程如圖2 所示：首先使用深度相機拍攝目標所在的三維空間，獲取整體環境的RGB-D 圖，其次利用Mask R-CNN模型將環境RGB-D 圖進行實例分割出目標，轉化為點云圖，再用Super4PCS 算法與參考目標進行點云配準，實現目標的六維姿態估計。

圖2 實驗流程圖

1 實例分割與點云配準

1.1 Mask R-CNN 模型

Mask R-CNN[9]是由 Fast R-CNN[10]、Faster R-CNN[11]逐漸改進而來的多任務卷積網絡模型，可以完成目標檢測、目標分類、像素級別的目標分割等多種任務。 在此之前，Faster R-CNN 網絡已實現一張圖片上物體的目標分類和目標檢測功能；全卷積神經網絡(Fully Convolutional Networks，FCN)可以得到一張圖片上物體的輪廓，并實現語義分割。 因此結合二者的功能，構建了Mask R-CNN 卷積神經網絡模型，如圖3 所示。 結構主要包括骨干部分的深度殘差網絡(Deep Residual Network，Res-Net)和特征圖金字塔網 絡(Feature Pyramid Networ-ks，FPN)，用 于特征提取；頭部部分的功能是目標分類、邊框回歸和Mask 預測。

圖 3 Mask R-CNN 結構圖

Mask R-CNN 模型主要有三點重要改進：第一，使用 ResNet 和 FPN 來實現圖像特征提取；第二，網絡輸出層添加二進制Mask 語義識別，對輸入網絡的任意尺寸的圖片，均可得到目標的邊框位置、分類結果和像素級的二進制Mask 預測，具體做法是在之前 Faster R-CNN 網絡中的邊框(Bounding Box，BB)識別分支旁邊并行添加了一個用于預測目標的Mask 分支，它是作用于每個感興趣區域(Region Of Interest，ROI)上的一個小型 FCN，用于預測 ROI 每個像素的類別，最終實現準確的實例分割；第三，改進了 Faster R-CNN 網絡 ROI Pooling 層在量化時會有像素對不準的問題，Mask R-CNN 網絡用ROI Align 層替換的 ROl Pooling 層，先把 feature map 線性插值到 14×14，再 pooling 到 7×7，消除了最終結果Mask 和實際位置之間的微小偏移。 Mask R-CNN模型功能強太，但速度和準確率并沒有降低，同時可以靈活地擴展，適用于本文的實例分割。

1.2 Super4PCS 配準算法

點云配準通過將具有重疊的兩兩點云迭代調整位姿直到一致，最終實現整體拼接。 假設點云P有 m 個點，點云 Q 有 n 個點，可以通過分別各選 3個點來確定二者對應關系。 最初窮舉搜索空間的算法復雜度為 O(m3n3)；RANSAC 算法[12]通過多次迭代選擇近似最優解，復雜度降為O(m3logn)。 而AIGER D等[13]提出的 4PCS 算法復雜度降低為 O(n2)，是目前速度和準確度均不錯的粗配準算法之一。

4PCS 算法的目標是尋找參考點云P 和目標點云Q 之間的一個最優剛性變換，使得兩片點云中存在符合最小允許范圍內的點最多，具體分三個步驟：

(1)在參考點云 P 中選擇點基 C={a，b，c，d}。 先用 RANSAC 算法選擇 3 個點，再挑能夠與這 3 個點組成的共面的第4 點，該點必須在兩片點云的重疊部分且不能距離其他3 點太近，共同組成點基C。

(2)在點云 Q 尋找任意點基 Di={a′，b′，c′，d′}，如圖4(b)所示，并且要求滿足利用仿射不變性質：

目標點云Q 中所有成立的Di行成的點基組合D，并剔除角度偏差不在閾值允許范圍內的一些冗余點基，如圖 4(b)中的{a′，b′，c″，d″}等。

圖4

(3)最后確定最佳的剛性變換矩陣 T。 計算上述獲得的點基C 與點基 D 中每個點基之間的剛體變換矩陣 Ti；把參考點云 P 做剛體變換 Ti得到 TiP 后，使得TiP 與Q 之間所有點的歐式距離平方和最小Ti，即為最佳剛體變換矩陣 T。

4PCS 算法的時間復雜度為二次，并且根據仿射不變性挑選出的點基D，需要窮盡計算每個點基Di中的角度來與點基C 比對，剔除冗余基代價過于昂貴。

Super4PCS[14]針對上述量點不足進行改進：

(1)在點云 Q 找到對應的對構建球殼來提取點對。 以任意為球心，以圖 4(a)中 ab 距離為半徑構建球殼，半徑的誤差限為ε，球殼上的點形成一個集合 S1。 同理以 cd 距離為半徑球殼上的點形成集合S2(圖5(a))，球殼中的每個單元存儲一系列向量方向, 通過二剖分球殼來陣列存儲所有落在球殼上的 點(圖 5(b))。

(2)同4PCS 算法，如圖 5(c)利用仿射不變性在集合S1和S2中找在誤差允許范圍內相等交點e1ij=e2ij并 記 錄 對 應 點 對{as1，bs1，cs2，ds2}，其 中 {e1ij，as1，bs1}?S1，{e2ij，cs2，ds2}?S2，并判斷兩個相交線對之間的角度 θ是否相等，最終找到同時滿足放射比例和交角θ 的點基礎 Di={a′，b′，c′，d′}。

上述得到近似全等的點基礎對和變換矩陣后，也還需要迭代尋找最優變換矩陣，Super4PCS 算法將復雜度降為O(n)，可以高效配準更高維度的點云數量，較快速實現目標的六維姿態估計。

2 實驗驗證

為了驗證該方法的有效性，本文通過真實采集的數據進行評估，實驗均在 CPU 為 Intel?CoreTMi5-10600KF 處 理 器 、16 GB 內 存 、GeForce RTX 3070的64 位Windows 10 系統中運行，采集數據使用Intel RealSense D435 深度相機，滿足實驗需求。

圖5

2.1 制作數據集

采集300 張發動機連桿的圖片作為實驗對象，對圖中的每個對象進行標注，保存對應生成的JSON文件。由于采集的數據集較小，進行數據集的增廣，可使訓練的模型有較強的泛化能力。 本實驗數據集的增廣主要是翻轉、加噪、模糊化、增減曝光四種方式；相應地也要把之前標注的JSON 文件進行增廣，增廣后的數據集共有2 000 張圖片。 本實驗選擇一部分(如圖 6 所示)作為 Mask R-CNN 模型分割的實驗對象，其余的作為數據集用于模型的訓練。

圖 6 原始 RGB 圖

2.2 Mask R-CNN 實例分割

本文的利用Mask R-CNN 模型獲取進行RGB-D圖像分割的流程主要包含以下幾個步驟：

(1)采集 RGB-D 數據集作訓練數據源，并對訓練樣本進行預處理，采用LabelMe 標注工具進行數據標注和Mask 制作；

(2)將其輸入到一個預訓練網絡 ResNet101 中，得到訓練樣本的特征圖，對該圖中每一點設定ROI(Region Of Interest)，從而獲得多個候選 ROI；

(3)將這些候選的 ROI 送入二值分類(前景或背景)和 BB(Bounding Box)回歸，將候選的 ROI 送入 RPN進行二分類(輸出為前景或背景)和 BB 回歸，過濾掉一部分候選的 ROI，對剩下 ROI 進行 ROI Align操作；

(4)對 這 些 ROI 進 行 分 類 (N 類 別 分 類)、BB 回 歸和 MASK 生成；

(5)重復步驟(4)，訓練完所有樣本，得到最終優化調整的分割模型。

Mask R-CNN 模型的輸入一般為 RBD 圖像，而RGB-D 圖像多一維深度信息，Mask R-CNN 模型網絡結構并不需要改變，只需要在開源代碼層面上增加一個通道來并行處理深度數據。 首先修改子類輸入的配置文件Config.py，圖像通道數IMAGE_CHANNEL_C-OUNT 為 4，平均像素值函數 MEAN_PIXEL 與 RGB值并行添加深度信息值；其次修改模型的輸入通道為4，并增添第一層網絡的深度信息的隨機權重參數，其他層的輸入參數和預訓練權重不變；最后由于上一步添加的參數是隨機的，因此需要加入訓練層獲得較好初始值。 通過修改 Mask R-CNN 模型的源碼直接對 RGB-D 圖像實例分割。 增廣后的數據集共有約 2 000 張圖片，訓練 30 個 epoch，每個 epoch時間約 25 min。 圖7 是有效的目標候選區域RGB 圖。

而本次實驗采用了單一參考目標，因此只需要分割出來單一的目標的RGB-D 圖像，作為分割后的目標與參考目標進行后續位姿配準。 從原圖中實例分割出來一個完整有效的目標的RGB-D 圖(如圖 8 所 示 )。

圖 8 目標 RGB-D 圖

2.3 點云配準

圖9(a)所示為已知位姿參考點云，由于實驗模型比較理想，可以對點云的數量處理為12 500，同時增加表面處理便于直觀顯示(如圖 9(b)所示)。用參考點云與分割后的目標點云進行配準，目標是實現基本重合，獲得兩者之間的變換矩陣，進而求得分割目標的六維位姿。

圖9

上述 Mask R-CNN 模型對 RGB-D 圖分割出目標的RGB-D 圖之后，需要轉換成點云圖，便于與參考點云配準工作。 RGB-D 圖中有 D435 深度相機到目標的距離值， 要轉換成目標表面的點云圖，核心是相機的坐標系與世界坐標系的關系轉化。本次實驗的設定的相機內參為camera_factor=1 000、camera_cx=346.502、camera_cy=274.623、camera_fx=648.791、camera_fy=649.105，不斷遍歷 RGB-D 圖并計算每個點的空間坐標后添加到點云中，并把離群的小片點云做去噪處理，便獲得了分割后目標的點云圖。

同樣為了直觀顯示，增加表面處理，點云數量的規模分為 23 501、38 934、47 558、48 215。 紅色目標為圖9(b)參考點云，綠色部分為圖8 中分割后目標RGB-D 圖轉化的目標點云，配準的結果如圖10所示，并記錄變換矩陣和配準時間。

圖10 分割目標點云與參考點云配準結果

本實驗繼續進行參考點云與未分割整體目標環境點云的配準實驗(圖 11)，同樣采用 Super4PCS 算法。 在多次實驗后并限定區域處理后，分別挑選出與分割后配準同一目標的配準結果，并作配準的時間對比。

圖11 原圖點云圖與參考點云配準結果

以上的配準結果包括變換矩陣和配準時間，變換矩陣可以實現目標六維位姿估計，配準時間用于做效率分析。

3 結論

如表1 所示，實驗結果記錄了分割前與分割后配準的點云時間對比。 由于分割后均為單一目標，且點云數量均處理到12 500，4 個分割目標點云配準時間 分別為 0.42 s、0.39 s、0.41 s、0.42 s，取均值0.41 s。

從表1 中可以看出對目標進行分割后配準時間顯著降低了60%~80% ， 因此先對目標采用Mask-R CNN 網絡實例分割目標， 再用 Super4PCS點云配準算法的方式來估計目標的六維位姿，該方法是高效可行的。

表1 分割后與原圖時間對比

由于實驗環境較為理想，僅用Super4PCS 粗配準就已經實現了基本重疊，后續也需要在更復雜場景下做多次驗證實驗。 基于上述分析，后續實驗可行優化方向：第一，考慮目標分割的時間代價，并考慮需要獲取位姿的目標數量達到如何規模時可采用分割后配準的方式；第二，增加環境的復雜性和目標的多樣性，通過Super4PCS 算法粗配準后再用ICP算法精配準，進一步分析配準效率。