在工業(yè)上的抓取作業(yè)中，多數(shù)情況下會有：待檢測物體類型單一、物體模型表面紋理較少、工件散亂且密集、金屬表面（油污、反光）影響成像效果等作業(yè)環(huán)境上的特殊性條件存在。針對不同的應用場景及作業(yè)需求，需要有的放矢地選擇一種合適的位姿估計方案，以提高后續(xù)機器人的抓取成功率及作業(yè)的整體效率。
現(xiàn)階段，針對剛性物體的位姿估計方法，大致可以歸為以下三類：
1 基于模板匹配
2 基于3d描述符（描述子）
3 基于學習（神經網絡/ end to end）
一、基于模板匹配的方式在可能的三維空間區(qū)域中，將待檢測物的三維模型進行不同視角下的渲染成像，提取各視角下物體的相關信息（如：輪廓、邊緣梯度、表面曲率等），對待檢測物體進行充分的采樣，提取足夠魯棒的模板集。匹配時，從場景中提取相關信息，并在建立好的模板集中進行查詢比對，找到相似度(zui)高的模板，并以該模板的位姿作為粗位姿，然后結合icp等方式進行位姿的細配準。
論文示例
《real-time 6d object pose estimation on cpu》[1]
該方法以模板匹配為基本原理，從特征集模板的構建、模板的存儲數(shù)據結構以及內存重組優(yōu)化等三個方面作出了改進和優(yōu)化工作，以提高配準的準確度及整個流程的速度。
01 模板特征集的構建
從多個視角對工件的cad模型進行投影成像，并對每一個投影數(shù)據構建pcof（perspectively cumulated orientation feature）特征集（每個視角對應一個模板位姿）。
特征集在構建時，同時使用投影點云的表面法向及輪廓梯度方向信息創(chuàng)建方向直方圖（共8個方向），并按照閾值將直方圖進行二值化（組頻率大于閾值的置為1），后用一個字節(jié)（byte）來存儲（一個byte共8個bit位，對應8個方向）。
方向直方圖示例，ori（二值化的方向直方圖），w（權重，以該直方圖中超過所設閾值的大頻率來表示）：
1,2兩像素點取自投影圖像的輪廓，以梯度方向構建直方圖
3,4兩像素點取自投影圖像表面，以法線方向構建方向直方圖
02 使用bpt數(shù)據結構存儲構建的模板
在對模型進行投影成像時，視角點的選取是通過將正二十面體（下圖中depth 0）不斷等分，終形成正1280面體（下圖中depth 3），然后均勻選取正1280面體的頂點作為模型投影成像的視角點。
填充構建”樹”型存儲結構時，從第3層（depth 3）開始，實際的成像視角點也是從第三層選取，依次從上一層找到分割時的“父節(jié)點（頂點）”，并使用當前層中所有屬于該“父節(jié)點”的模板位姿的均值作為“父節(jié)點”的位姿；
遞歸處理，直至第0層（depth 0），以此創(chuàng)建一個4層的、由“粗”到“細”的模板位姿樹（bpt（balanced pose tree））
將創(chuàng)建的模板按照“樹”型結構進行分層存儲（共四層），查找時，實現(xiàn)由“粗”到“細”的模板搜索方式，以便加速模板的匹配查找，得到粗位姿。
03 利用simd技術實現(xiàn)內存重組
結合“樹”型數(shù)據存儲結構，利用simd（single instruction multiple data）技術實現(xiàn)內存重組，以達到加速的目的。
該方法示例效果：
該方法分別在桌面場景（左圖）
及無序抓取場景（右圖）下的測試效果
二、基于3d描述符（描述子）三組對應點即可解析得到兩模型之間的對應位姿，所以，利用法向量、點間距等數(shù)學語言描述出模型表面的線條、邊緣等多種局部特征，進而找出準確的對應點。在得到匹配的點對之后，再結合隨機采樣一致性（ransac）或者霍夫投票（hough voting）等方式得到局部(zui)優(yōu)解，以此作為模型配準的粗位姿，后利用icp等方式進行精化得到終結果。
論文示例
《model globally, match locally: efficient and robust 3d object recognition》[2]
該方法提出了一種新的局部描述符——ppf（point pair featrue）,通過對待匹配工件的點云中，任意兩個數(shù)據點之間構建ppf特征，終形成一個該工件模型整體的ppf特征集。在實際匹配時，從場景點云中選取參考點，以該參考點為中心，與周圍的數(shù)據點構建ppf特征，并將該特征與模型特征集中的數(shù)據作匹配，然后利用霍夫投票的形式，得到場景點云中該參考點所屬工件的粗位姿，后再利用icp等方式進行位姿的細化配準。
01 構建模型的ppf特征集（model globally）
整體建模，構建對模型的全局描述，其中“globally”是指對整個模型點云進行特征（ppf，point pair feature）的計算，然后將所有計算得到的特征集存儲在hash表中，以供后續(xù)匹配時查詢使用。（離線階段完成）
model globally 的本質是通過定義 point pair feature，來構建特征矢量的集合以及每個特征矢量對應的“點對”集，作為 global model desciption。
ppf 描述了兩個有向點（oriented points）的相對位置和姿態(tài)。假設有兩個點 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表索引可能對應多個特征點對
02 通過hash表查詢，進行投票匹配（match locally）
局部匹配，從場景點云中選取參考點，并與周圍數(shù)據點構建ppf特征，然后到hash表中查找是否存在一致的特征值，由于是以場景的單個點對特征到模型的全局描述（hash table）中進行匹配，所以稱為“局部”匹配。
其中，恒定坐標系為整個匹配流程的核心概念，由于模型及場景點云中每個數(shù)據點都是法線已知，故將“模型點對”與“場景點對”各自旋轉，使其參考點的法向與恒定坐標系x軸已知，且參考點的位置與恒定坐標系原點重合，得到各自的變換矩陣（rt） ts-g和tm->g；當“場景點對”與“模型點對”同處于恒定坐標系下時，將“模型點對”繞x軸旋轉，使其與“場景點對”重合，得到旋轉角度值α，接著以該模型點的索引與α值，在累計投票表中進行投票。（參考圖示如下）
ppf(mr, mi) 為“模型點對”,mr為模型參考點
ppf(sr, si) 為“場景點對”，sr為場景參考點
ppf特征中的法線方向，皆為參考點的法線方向
構建的“場景點對”需要與有相同hash索引的所有“模型點對”進行上述投票處理，故每一個“場景點對”都會生成一張投票表，其中，行數(shù)與模型點數(shù)相同，且與模型索引值一一對應；列數(shù)為劃分的角度數(shù)目，該方法中共劃分為30份，以（π/15）為角度間隔。（參考圖示如下）
從hash表中搜索與“場景點對”具有相同hash索引的“模型點對”，然后進行累計投票
03 提取粗位姿
根據上述流程中統(tǒng)計得到的累計投分表尋找分值大于所設閾值的位置。由于我們已經得到了“場景點對”及“模型點對”各自旋轉到恒定坐標系下的變換矩陣，以及“模型點對”繞x軸旋轉到“場景點對”的旋轉角度α，故可以計算出模型旋轉到場景中工件位姿的變換矩陣，即：
04 位姿聚類
每個參考點都可能會返回多個位姿（分值相同或多個分值大于所設閾值的位姿），對所有返回的位姿進行聚類，將位姿差異不超過所設閾值的位姿歸為同一類，然后對每個聚類中的位姿取均值，依次代表該聚類的位姿，其分值為聚類中所有位姿的總分值，后比較所有聚類的分值，將分值(zui)高的聚類位姿作為配準得到的位姿輸出。
該方法示例效果：
在遮擋及噪聲場景下的測試
經物體識別與位姿估計后，模型以網格的形式展現(xiàn)，以作對比
三、基于學習（神經網絡）這類方法也可以叫作基于回歸的方法。從模型的三維點云數(shù)據或投影到多個視角下的的圖像數(shù)據中尋找位姿和圖像之間的內在聯(lián)(lian)系。常見的基于學習的方法可分為兩類，一類是直接回歸出物體的6d位姿（語義分割->translation估計-> rotation估計）；另一類方法是先估計3d keypoint在2d圖像上的投影點，然后使用pnp（perspective-n-point）等算法方式對位姿進行恢復。
論文示例
《posecnn: a convolutional neural network for 6d object pose estimation in cluttered scenes》[3]
在傳統(tǒng)的物體識別與位姿估計方式中，存在對于弱紋理、對稱以及有遮擋的物體的識別及位姿估計能力相對較弱的情況，針對這些問題，該方法提出一種端到端（end to end）的用于6d目標姿態(tài)估計的新型卷積神經網絡posecnn。posecnn通過在圖像中定位物體的中心并預測其與攝像機的距離來估計物體的三維平移。通過回歸到四元數(shù)(w,x,y,z)表示來估計物體的三維旋轉。其主要任務有三項：語義分割、3d平移估計、3d旋轉估計。此外，還引入了一種用于對稱物體姿態(tài)估計的新訓練損失函數(shù)shapematch-loss，并提供了一個大規(guī)模的rgb-d視頻數(shù)據集（ycb-video dataset），用于6d物體姿態(tài)估計。
網絡架構
該網絡共含有三個分支：語義分割分支、位置估計分支、姿態(tài)估計分支，每個分支都有一個loss函數(shù)，共三個。
01 位置估計分支
通過3d位置估計獲得平移矩陣t=（tx,ty,tz）（目標物體在相機坐標系下的坐標），一種簡單的估計t的方法是直接將圖像特征回歸到t，但是由于目標可能出現(xiàn)在圖像上的任意位置，因此這種方式是不可推廣的。并且不能處理同類的多個目標實例。該方法通過定位2d圖像的目標中心并估計目標到相機的距離來估計三維平移量。
（1）目標對象2d中心的預測（hough voting）
該層采用像素方式的語義標記結果和中心回歸結果作為輸入。對于每一個物體，首先計算圖像中的每個位置的投票得分，其分值表明了相應的圖像位置是物體的中心的可能性大小。物體的每個像素都會根據網絡預測添加圖像位置的投票，在處理完物體的所有像素后可以獲得所有圖像位置的投票分數(shù)，后選擇(zui)高分的位置作為物體中心。
通過如下公式，可以恢復得到位置平移矩陣中的tx和ty：
fx、fy表示相機焦距，（px，py）是主點，c是目標的二維中心
（2）深度預測tz
生成了一系列目標中心之后，將投票給目標中心的像素視為該中心的初始值。然后，中心深度tz的預測，被簡單地計算為由初始值預測的深度的平均值。
02 姿態(tài)估計分支
在hough投票層可以預測得到的物體對象2d bbox，同時利用兩個roi池化層對網絡*階段生成的視覺特征進行剪裁和池化，然后進行3d旋轉回歸，得到目標對象的旋轉變換。
為了對四元數(shù)進行回歸訓練，該方法提出兩個損失函數(shù)：
1、poseloss（ploss），正確模型姿態(tài)上的點與相應使用估計方向上的點的平均平方誤差。
2、shapematch-loss（sloss）,不需要對稱規(guī)范，估計點與真值近點的損失測量。
該方法示例效果：
分別在ycb-video和occludedlinemod數(shù)據集上作測試，
由上至下分別為：輸入數(shù)據、語義分割及中心點標注后的結果、
僅使用rgb數(shù)據、使用rgb-d數(shù)據并使用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.
更多機器視覺產品信息與應用說明，機器視覺算法，應用技巧，解決方案以及相關領域的技術分享，歡迎繼續(xù)關注“漢振智能”....