一種基于實(shí)例分割和點(diǎn)云配準(zhǔn)的六維位姿估計(jì)方法

侯大偉

(中國(guó)科學(xué)技術(shù)太學(xué) 信息科學(xué)技術(shù)學(xué)院，安徽 合肥230026)

0 引言

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

以上兩片點(diǎn)云配準(zhǔn)的過(guò)程，本質(zhì)上得到的是兩片點(diǎn)云之間相對(duì)位姿的變換矩陣。假設(shè)其中一片點(diǎn)云參考目標(biāo)，即相對(duì)世界坐標(biāo)系的位姿參數(shù)均已知，便可推理得到另一片點(diǎn)云的位姿，從而實(shí)現(xiàn)目標(biāo)點(diǎn)云的位姿估計(jì)。

圖1

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

本文采用的是先分割RGB-D 圖后進(jìn)行點(diǎn)云配準(zhǔn)來(lái)估計(jì)六維姿態(tài)的方法。 整體的流程如圖2 所示：首先使用深度相機(jī)拍攝目標(biāo)所在的三維空間，獲取整體環(huán)境的RGB-D 圖，其次利用Mask R-CNN模型將環(huán)境RGB-D 圖進(jìn)行實(shí)例分割出目標(biāo)，轉(zhuǎn)化為點(diǎn)云圖，再用Super4PCS 算法與參考目標(biāo)進(jìn)行點(diǎn)云配準(zhǔn)，實(shí)現(xiàn)目標(biāo)的六維姿態(tài)估計(jì)。

圖2 實(shí)驗(yàn)流程圖

1 實(shí)例分割與點(diǎn)云配準(zhǔn)

1.1 Mask R-CNN 模型

Mask R-CNN[9]是由 Fast R-CNN[10]、Faster R-CNN[11]逐漸改進(jìn)而來(lái)的多任務(wù)卷積網(wǎng)絡(luò)模型，可以完成目標(biāo)檢測(cè)、目標(biāo)分類(lèi)、像素級(jí)別的目標(biāo)分割等多種任務(wù)。 在此之前，F(xiàn)aster R-CNN 網(wǎng)絡(luò)已實(shí)現(xiàn)一張圖片上物體的目標(biāo)分類(lèi)和目標(biāo)檢測(cè)功能；全卷積神經(jīng)網(wǎng)絡(luò)(Fully Convolutional Networks，F(xiàn)CN)可以得到一張圖片上物體的輪廓，并實(shí)現(xiàn)語(yǔ)義分割。 因此結(jié)合二者的功能，構(gòu)建了Mask R-CNN 卷積神經(jīng)網(wǎng)絡(luò)模型，如圖3 所示。 結(jié)構(gòu)主要包括骨干部分的深度殘差網(wǎng)絡(luò)(Deep Residual Network，Res-Net)和特征圖金字塔網(wǎng) 絡(luò)(Feature Pyramid Networ-ks，F(xiàn)PN)，用 于特征提取；頭部部分的功能是目標(biāo)分類(lèi)、邊框回歸和Mask 預(yù)測(cè)。

圖 3 Mask R-CNN 結(jié)構(gòu)圖

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

1.2 Super4PCS 配準(zhǔn)算法

點(diǎn)云配準(zhǔn)通過(guò)將具有重疊的兩兩點(diǎn)云迭代調(diào)整位姿直到一致，最終實(shí)現(xiàn)整體拼接。 假設(shè)點(diǎn)云P有 m 個(gè)點(diǎn)，點(diǎn)云 Q 有 n 個(gè)點(diǎn)，可以通過(guò)分別各選 3個(gè)點(diǎn)來(lái)確定二者對(duì)應(yīng)關(guān)系。 最初窮舉搜索空間的算法復(fù)雜度為 O(m3n3)；RANSAC 算法[12]通過(guò)多次迭代選擇近似最優(yōu)解，復(fù)雜度降為O(m3logn)。 而AIGER D等[13]提出的 4PCS 算法復(fù)雜度降低為 O(n2)，是目前速度和準(zhǔn)確度均不錯(cuò)的粗配準(zhǔn)算法之一。

4PCS 算法的目標(biāo)是尋找參考點(diǎn)云P 和目標(biāo)點(diǎn)云Q 之間的一個(gè)最優(yōu)剛性變換，使得兩片點(diǎn)云中存在符合最小允許范圍內(nèi)的點(diǎn)最多，具體分三個(gè)步驟：

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

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

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

圖4

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

4PCS 算法的時(shí)間復(fù)雜度為二次，并且根據(jù)仿射不變性挑選出的點(diǎn)基D，需要窮盡計(jì)算每個(gè)點(diǎn)基Di中的角度來(lái)與點(diǎn)基C 比對(duì)，剔除冗余基代價(jià)過(guò)于昂貴。

Super4PCS[14]針對(duì)上述量點(diǎn)不足進(jìn)行改進(jìn)：

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

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

上述得到近似全等的點(diǎn)基礎(chǔ)對(duì)和變換矩陣后，也還需要迭代尋找最優(yōu)變換矩陣，Super4PCS 算法將復(fù)雜度降為O(n)，可以高效配準(zhǔn)更高維度的點(diǎn)云數(shù)量，較快速實(shí)現(xiàn)目標(biāo)的六維姿態(tài)估計(jì)。

2 實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證該方法的有效性，本文通過(guò)真實(shí)采集的數(shù)據(jù)進(jìn)行評(píng)估，實(shí)驗(yàn)均在 CPU 為 Intel?CoreTMi5-10600KF 處 理 器 、16 GB 內(nèi) 存 、GeForce RTX 3070的64 位Windows 10 系統(tǒng)中運(yùn)行，采集數(shù)據(jù)使用Intel RealSense D435 深度相機(jī)，滿足實(shí)驗(yàn)需求。

圖5

2.1 制作數(shù)據(jù)集

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

圖 6 原始 RGB 圖

2.2 Mask R-CNN 實(shí)例分割

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

(1)采集 RGB-D 數(shù)據(jù)集作訓(xùn)練數(shù)據(jù)源，并對(duì)訓(xùn)練樣本進(jìn)行預(yù)處理，采用LabelMe 標(biāo)注工具進(jìn)行數(shù)據(jù)標(biāo)注和Mask 制作；

(2)將其輸入到一個(gè)預(yù)訓(xùn)練網(wǎng)絡(luò) ResNet101 中，得到訓(xùn)練樣本的特征圖，對(duì)該圖中每一點(diǎn)設(shè)定ROI(Region Of Interest)，從而獲得多個(gè)候選 ROI；

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

(4)對(duì) 這 些 ROI 進(jìn) 行 分 類(lèi) (N 類(lèi) 別 分 類(lèi))、BB 回 歸和 MASK 生成；

(5)重復(fù)步驟(4)，訓(xùn)練完所有樣本，得到最終優(yōu)化調(diào)整的分割模型。

Mask R-CNN 模型的輸入一般為 RBD 圖像，而RGB-D 圖像多一維深度信息，Mask R-CNN 模型網(wǎng)絡(luò)結(jié)構(gòu)并不需要改變，只需要在開(kāi)源代碼層面上增加一個(gè)通道來(lái)并行處理深度數(shù)據(jù)。 首先修改子類(lèi)輸入的配置文件Config.py，圖像通道數(shù)IMAGE_CHANNEL_C-OUNT 為 4，平均像素值函數(shù) MEAN_PIXEL 與 RGB值并行添加深度信息值；其次修改模型的輸入通道為4，并增添第一層網(wǎng)絡(luò)的深度信息的隨機(jī)權(quán)重參數(shù)，其他層的輸入?yún)?shù)和預(yù)訓(xùn)練權(quán)重不變；最后由于上一步添加的參數(shù)是隨機(jī)的，因此需要加入訓(xùn)練層獲得較好初始值。 通過(guò)修改 Mask R-CNN 模型的源碼直接對(duì) RGB-D 圖像實(shí)例分割。 增廣后的數(shù)據(jù)集共有約 2 000 張圖片，訓(xùn)練 30 個(gè) epoch，每個(gè) epoch時(shí)間約 25 min。 圖7 是有效的目標(biāo)候選區(qū)域RGB 圖。

而本次實(shí)驗(yàn)采用了單一參考目標(biāo)，因此只需要分割出來(lái)單一的目標(biāo)的RGB-D 圖像，作為分割后的目標(biāo)與參考目標(biāo)進(jìn)行后續(xù)位姿配準(zhǔn)。 從原圖中實(shí)例分割出來(lái)一個(gè)完整有效的目標(biāo)的RGB-D 圖(如圖 8 所 示 )。

圖 8 目標(biāo) RGB-D 圖

2.3 點(diǎn)云配準(zhǔn)

圖9(a)所示為已知位姿參考點(diǎn)云，由于實(shí)驗(yàn)?zāi)Ｐ捅容^理想，可以對(duì)點(diǎn)云的數(shù)量處理為12 500，同時(shí)增加表面處理便于直觀顯示(如圖 9(b)所示)。用參考點(diǎn)云與分割后的目標(biāo)點(diǎn)云進(jìn)行配準(zhǔn)，目標(biāo)是實(shí)現(xiàn)基本重合，獲得兩者之間的變換矩陣，進(jìn)而求得分割目標(biāo)的六維位姿。

圖9

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

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

圖10 分割目標(biāo)點(diǎn)云與參考點(diǎn)云配準(zhǔn)結(jié)果

本實(shí)驗(yàn)繼續(xù)進(jìn)行參考點(diǎn)云與未分割整體目標(biāo)環(huán)境點(diǎn)云的配準(zhǔn)實(shí)驗(yàn)(圖 11)，同樣采用 Super4PCS 算法。 在多次實(shí)驗(yàn)后并限定區(qū)域處理后，分別挑選出與分割后配準(zhǔn)同一目標(biāo)的配準(zhǔn)結(jié)果，并作配準(zhǔn)的時(shí)間對(duì)比。

圖11 原圖點(diǎn)云圖與參考點(diǎn)云配準(zhǔn)結(jié)果

以上的配準(zhǔn)結(jié)果包括變換矩陣和配準(zhǔn)時(shí)間，變換矩陣可以實(shí)現(xiàn)目標(biāo)六維位姿估計(jì)，配準(zhǔn)時(shí)間用于做效率分析。

3 結(jié)論

如表1 所示，實(shí)驗(yàn)結(jié)果記錄了分割前與分割后配準(zhǔn)的點(diǎn)云時(shí)間對(duì)比。 由于分割后均為單一目標(biāo)，且點(diǎn)云數(shù)量均處理到12 500，4 個(gè)分割目標(biāo)點(diǎn)云配準(zhǔn)時(shí)間 分別為 0.42 s、0.39 s、0.41 s、0.42 s，取均值0.41 s。

從表1 中可以看出對(duì)目標(biāo)進(jìn)行分割后配準(zhǔn)時(shí)間顯著降低了60%~80% ， 因此先對(duì)目標(biāo)采用Mask-R CNN 網(wǎng)絡(luò)實(shí)例分割目標(biāo)， 再用 Super4PCS點(diǎn)云配準(zhǔn)算法的方式來(lái)估計(jì)目標(biāo)的六維位姿，該方法是高效可行的。

表1 分割后與原圖時(shí)間對(duì)比

由于實(shí)驗(yàn)環(huán)境較為理想，僅用Super4PCS 粗配準(zhǔn)就已經(jīng)實(shí)現(xiàn)了基本重疊，后續(xù)也需要在更復(fù)雜場(chǎng)景下做多次驗(yàn)證實(shí)驗(yàn)。 基于上述分析，后續(xù)實(shí)驗(yàn)可行優(yōu)化方向：第一，考慮目標(biāo)分割的時(shí)間代價(jià)，并考慮需要獲取位姿的目標(biāo)數(shù)量達(dá)到如何規(guī)模時(shí)可采用分割后配準(zhǔn)的方式；第二，增加環(huán)境的復(fù)雜性和目標(biāo)的多樣性，通過(guò)Super4PCS 算法粗配準(zhǔn)后再用ICP算法精配準(zhǔn)，進(jìn)一步分析配準(zhǔn)效率。