【引言】

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

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

1 基于模板匹配

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

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

一、基于模板匹配的方式

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

論文示例

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

該方法示例效果：

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

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

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

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

論文示例

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

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

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

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

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

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

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

m1為該P(yáng)PF(m1, m2)中的參考點(diǎn)，n1,n2分別為各自的法向量;

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

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

02 通過Hash表查詢，進(jìn)行投票匹配（Match Locally）

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

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

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

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

PPF特征中的法線方向，皆為參考點(diǎn)的法線方向

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

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

03 提取粗位姿

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

04 位姿聚類

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

該方法示例效果：

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

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

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

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

論文示例

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

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

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

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

01 位置估計(jì)分支

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

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

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

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

fx、fy表示相機(jī)焦距，（px，py）是主點(diǎn)，c是目標(biāo)的二維中心

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

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

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

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

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

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

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

該方法示例效果：

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

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

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

參考文獻(xiàn)：

[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.

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