【引言】

在工業(yè)上的抓取作業(yè)中，多數(shù)情況下會有：待檢測物體類型單一、物體模型表面紋理較少、工件散亂且密集、金屬表面（油污、反光）影響成像效果等作業(yè)環(huán)境上的特殊性條件存在。針對不同的應(yīng)用場景及作業(yè)需求，需要有的放矢地選擇一種合適的位姿估計方案，以提高后續(xù)機器人的抓取成功率及作業(yè)的整體效率。

現(xiàn)階段，針對剛性物體的位姿估計方法，大致可以歸為以下三類：

1 基于模板匹配

2 基于3D描述符（描述子）

3 基于學(xué)習(xí)（神經(jīng)網(wǎng)絡(luò)/ end to end）

一、基于模板匹配的方式

在可能的三維空間區(qū)域中，將待檢測物的三維模型進行不同視角下的渲染成像，提取各視角下物體的相關(guān)信息（如：輪廓、邊緣梯度、表面曲率等），對待檢測物體進行充分的采樣，提取足夠魯棒的模板集。匹配時，從場景中提取相關(guān)信息，并在建立好的模板集中進行查詢比對，找到相似度(zui)高的模板，并以該模板的位姿作為粗位姿，然后結(jié)合ICP等方式進行位姿的細配準。

論文示例

《Real-Time 6D Object Pose Estimation on CPU》[1]

該方法以模板匹配為基本原理，從特征集模板的構(gòu)建、模板的存儲數(shù)據(jù)結(jié)構(gòu)以及內(nèi)存重組優(yōu)化等三個方面作出了改進和優(yōu)化工作，以提高配準的準確度及整個流程的速度。

01 模板特征集的構(gòu)建

從多個視角對工件的CAD模型進行投影成像，并對每一個投影數(shù)據(jù)構(gòu)建PCOF（Perspectively Cumulated Orientation Feature）特征集（每個視角對應(yīng)一個模板位姿）。

特征集在構(gòu)建時，同時使用投影點云的表面法向及輪廓梯度方向信息創(chuàng)建方向直方圖（共8個方向），并按照閾值將直方圖進行二值化（組頻率大于閾值的置為1），后用一個字節(jié)（byte）來存儲（一個byte共8個bit位，對應(yīng)8個方向）。

方向直方圖示例，ori（二值化的方向直方圖），w（權(quán)重，以該直方圖中超過所設(shè)閾值的大頻率來表示）：

1,2兩像素點取自投影圖像的輪廓，以梯度方向構(gòu)建直方圖

3,4兩像素點取自投影圖像表面，以法線方向構(gòu)建方向直方圖

02 使用BPT數(shù)據(jù)結(jié)構(gòu)存儲構(gòu)建的模板

在對模型進行投影成像時，視角點的選取是通過將正二十面體（下圖中depth 0）不斷等分，終形成正1280面體（下圖中depth 3），然后均勻選取正1280面體的頂點作為模型投影成像的視角點。

填充構(gòu)建”樹”型存儲結(jié)構(gòu)時，從第3層（depth 3）開始，實際的成像視角點也是從第三層選取，依次從上一層找到分割時的“父節(jié)點（頂點）”，并使用當前層中所有屬于該“父節(jié)點”的模板位姿的均值作為“父節(jié)點”的位姿；

遞歸處理，直至第0層（depth 0），以此創(chuàng)建一個4層的、由“粗”到“細”的模板位姿樹（BPT（Balanced Pose Tree））

將創(chuàng)建的模板按照“樹”型結(jié)構(gòu)進行分層存儲（共四層），查找時，實現(xiàn)由“粗”到“細”的模板搜索方式，以便加速模板的匹配查找，得到粗位姿。

03 利用SIMD技術(shù)實現(xiàn)內(nèi)存重組

結(jié)合“樹”型數(shù)據(jù)存儲結(jié)構(gòu)，利用SIMD（Single Instruction Multiple Data）技術(shù)實現(xiàn)內(nèi)存重組，以達到加速的目的。

該方法示例效果：

該方法分別在桌面場景（左圖）

及無序抓取場景（右圖）下的測試效果

二、基于3D描述符（描述子）

三組對應(yīng)點即可解析得到兩模型之間的對應(yīng)位姿，所以，利用法向量、點間距等數(shù)學(xué)語言描述出模型表面的線條、邊緣等多種局部特征，進而找出準確的對應(yīng)點。在得到匹配的點對之后，再結(jié)合隨機采樣一致性（RANSAC）或者霍夫投票（Hough voting）等方式得到局部(zui)優(yōu)解，以此作為模型配準的粗位姿，后利用ICP等方式進行精化得到終結(jié)果。

論文示例

《Model Globally, Match Locally: Efficient and Robust 3D Object Recognition》[2]

該方法提出了一種新的局部描述符——PPF（Point Pair Featrue）,通過對待匹配工件的點云中，任意兩個數(shù)據(jù)點之間構(gòu)建PPF特征，終形成一個該工件模型整體的PPF特征集。在實際匹配時，從場景點云中選取參考點，以該參考點為中心，與周圍的數(shù)據(jù)點構(gòu)建PPF特征，并將該特征與模型特征集中的數(shù)據(jù)作匹配，然后利用霍夫投票的形式，得到場景點云中該參考點所屬工件的粗位姿，后再利用ICP等方式進行位姿的細化配準。

01 構(gòu)建模型的PPF特征集（Model Globally）

整體建模，構(gòu)建對模型的全局描述，其中“Globally”是指對整個模型點云進行特征（PPF，Point Pair Feature）的計算，然后將所有計算得到的特征集存儲在Hash表中，以供后續(xù)匹配時查詢使用。（離線階段完成）

Model Globally 的本質(zhì)是通過定義 Point Pair Feature，來構(gòu)建特征矢量的集合以及每個特征矢量對應(yīng)的“點對”集，作為 Global Model Desciption。

PPF 描述了兩個有向點（oriented points）的相對位置和姿態(tài)。假設(shè)有兩個點 m1 和 m2 ，法向量（normals）分別為 n1 和 n2 ，d = m2 - m1 （有向線段） ，則 PPF 定義如下：

（ 即 F(m1,m2) ≠ F(m2,m1) ）

m1為該PPF(m1, m2)中的參考點，n1,n2分別為各自的法向量;

點對特征PPF(m1,m2) = (F1, F2,F3,F4)；

由于存在具有相同PPF特征的多個點對，所以同一個hash表索引可能對應(yīng)多個特征點對

02 通過Hash表查詢，進行投票匹配（Match Locally）

局部匹配，從場景點云中選取參考點，并與周圍數(shù)據(jù)點構(gòu)建PPF特征，然后到hash表中查找是否存在一致的特征值，由于是以場景的單個點對特征到模型的全局描述（hash table）中進行匹配，所以稱為“局部”匹配。

其中，恒定坐標系為整個匹配流程的核心概念，由于模型及場景點云中每個數(shù)據(jù)點都是法線已知，故將“模型點對”與“場景點對”各自旋轉(zhuǎn)，使其參考點的法向與恒定坐標系X軸已知，且參考點的位置與恒定坐標系原點重合，得到各自的變換矩陣（RT） Ts-g和Tm->g；當“場景點對”與“模型點對”同處于恒定坐標系下時，將“模型點對”繞X軸旋轉(zhuǎn)，使其與“場景點對”重合，得到旋轉(zhuǎn)角度值α，接著以該模型點的索引與α值，在累計投票表中進行投票。（參考圖示如下）

PPF(mr, mi) 為“模型點對”,mr為模型參考點

PPF(sr, si) 為“場景點對”，sr為場景參考點

PPF特征中的法線方向，皆為參考點的法線方向

構(gòu)建的“場景點對”需要與有相同Hash索引的所有“模型點對”進行上述投票處理，故每一個“場景點對”都會生成一張投票表，其中，行數(shù)與模型點數(shù)相同，且與模型索引值一一對應(yīng)；列數(shù)為劃分的角度數(shù)目，該方法中共劃分為30份，以（π/15）為角度間隔。（參考圖示如下）

從Hash表中搜索與“場景點對”具有相同Hash索引的“模型點對”，然后進行累計投票

03 提取粗位姿

根據(jù)上述流程中統(tǒng)計得到的累計投分表尋找分值大于所設(shè)閾值的位置。由于我們已經(jīng)得到了“場景點對”及“模型點對”各自旋轉(zhuǎn)到恒定坐標系下的變換矩陣，以及“模型點對”繞X軸旋轉(zhuǎn)到“場景點對”的旋轉(zhuǎn)角度α，故可以計算出模型旋轉(zhuǎn)到場景中工件位姿的變換矩陣，即：

04 位姿聚類

每個參考點都可能會返回多個位姿（分值相同或多個分值大于所設(shè)閾值的位姿），對所有返回的位姿進行聚類，將位姿差異不超過所設(shè)閾值的位姿歸為同一類，然后對每個聚類中的位姿取均值，依次代表該聚類的位姿，其分值為聚類中所有位姿的總分值，后比較所有聚類的分值，將分值(zui)高的聚類位姿作為配準得到的位姿輸出。

該方法示例效果：

在遮擋及噪聲場景下的測試

經(jīng)物體識別與位姿估計后，模型以網(wǎng)格的形式展現(xiàn)，以作對比

三、基于學(xué)習(xí)（神經(jīng)網(wǎng)絡(luò)）

這類方法也可以叫作基于回歸的方法。從模型的三維點云數(shù)據(jù)或投影到多個視角下的的圖像數(shù)據(jù)中尋找位姿和圖像之間的內(nèi)在聯(lián)(lian)系。常見的基于學(xué)習(xí)的方法可分為兩類，一類是直接回歸出物體的6D位姿（語義分割->Translation估計-> Rotation估計）；另一類方法是先估計3D KeyPoint在2D圖像上的投影點，然后使用PnP（Perspective-n-Point）等算法方式對位姿進行恢復(fù)。

論文示例

《PoseCNN: A convolutional neural network for 6d object pose estimation in cluttered scenes》[3]

在傳統(tǒng)的物體識別與位姿估計方式中，存在對于弱紋理、對稱以及有遮擋的物體的識別及位姿估計能力相對較弱的情況，針對這些問題，該方法提出一種端到端（end to end）的用于6D目標姿態(tài)估計的新型卷積神經(jīng)網(wǎng)絡(luò)PoseCNN。PoseCNN通過在圖像中定位物體的中心并預(yù)測其與攝像機的距離來估計物體的三維平移。通過回歸到四元數(shù)(w,x,y,z)表示來估計物體的三維旋轉(zhuǎn)。其主要任務(wù)有三項：語義分割、3D平移估計、3D旋轉(zhuǎn)估計。此外，還引入了一種用于對稱物體姿態(tài)估計的新訓(xùn)練損失函數(shù)ShapeMatch-Loss，并提供了一個大規(guī)模的RGB-D視頻數(shù)據(jù)集（YCB-Video dataset），用于6D物體姿態(tài)估計。

網(wǎng)絡(luò)架構(gòu)

該網(wǎng)絡(luò)共含有三個分支：語義分割分支、位置估計分支、姿態(tài)估計分支，每個分支都有一個loss函數(shù)，共三個。

01 位置估計分支

通過3D位置估計獲得平移矩陣T=（Tx,Ty,Tz）（目標物體在相機坐標系下的坐標），一種簡單的估計T的方法是直接將圖像特征回歸到T，但是由于目標可能出現(xiàn)在圖像上的任意位置，因此這種方式是不可推廣的。并且不能處理同類的多個目標實例。該方法通過定位2D圖像的目標中心并估計目標到相機的距離來估計三維平移量。

（1）目標對象2D中心的預(yù)測（Hough Voting）

該層采用像素方式的語義標記結(jié)果和中心回歸結(jié)果作為輸入。對于每一個物體，首先計算圖像中的每個位置的投票得分，其分值表明了相應(yīng)的圖像位置是物體的中心的可能性大小。物體的每個像素都會根據(jù)網(wǎng)絡(luò)預(yù)測添加圖像位置的投票，在處理完物體的所有像素后可以獲得所有圖像位置的投票分數(shù)，后選擇(zui)高分的位置作為物體中心。

通過如下公式，可以恢復(fù)得到位置平移矩陣中的Tx和Ty：

fx、fy表示相機焦距，（px，py）是主點，c是目標的二維中心

（2）深度預(yù)測Tz

生成了一系列目標中心之后，將投票給目標中心的像素視為該中心的初始值。然后，中心深度Tz的預(yù)測，被簡單地計算為由初始值預(yù)測的深度的平均值。

02 姿態(tài)估計分支

在Hough投票層可以預(yù)測得到的物體對象2D BBox，同時利用兩個RoI池化層對網(wǎng)絡(luò)*階段生成的視覺特征進行剪裁和池化，然后進行3D旋轉(zhuǎn)回歸，得到目標對象的旋轉(zhuǎn)變換。

為了對四元數(shù)進行回歸訓(xùn)練，該方法提出兩個損失函數(shù)：

1、PoseLoss（PLoss），正確模型姿態(tài)上的點與相應(yīng)使用估計方向上的點的平均平方誤差。

2、ShapeMatch-Loss（SLoss）,不需要對稱規(guī)范，估計點與真值近點的損失測量。

該方法示例效果：

分別在YCB-Video和OccludedLINEMOD數(shù)據(jù)集上作測試，

由上至下分別為：輸入數(shù)據(jù)、語義分割及中心點標注后的結(jié)果、

僅使用RGB數(shù)據(jù)、使用RGB-D數(shù)據(jù)并使用ICP進行位姿細化

參考文獻：

[1] Yoshinori Konishi, Kosuke Hattori, and Manabu Hashimoto. Real-time 6d object pose estimation on cpu. IEEE/RSJ International Conference on Inte-lligent Robots and Systems (IROS), Macau, China, 2019, pp. 3451-3458

[2] B. Drost, M. Ulrich, N. Navab, and S. Ilic. Model globally, match locally: Efficient and robust 3d object recognition. IEEE Computer Society Conference on Computer Vision and Pattern Recognition • July 2010

[3] Yu Xiang, Tanner Schmidt, Venkatraman Narayanan, and Dieter Fox. PoseCNN: A convolutional neural network for 6d object pose estimation in cluttered scenes. arXiv preprint arXiv:1711.00199, 2017.

更多機器視覺產(chǎn)品信息與應(yīng)用說明，機器視覺算法，應(yīng)用技巧，解決方案以及相關(guān)領(lǐng)域的技術(shù)分享，歡迎繼續(xù)關(guān)注“漢振智能”....