引言
在工業上的抓取作業中，多數情況下會有：待檢測物體類型單一、物體模型表面紋理較少、工件散亂且密集、金屬表面(油污、反光)影響成像效果等作業環境上的特殊性條件存在。針對不同的應用場景及作業需求，需要有的放矢地選擇一種合适的位姿估計方案，以提高後續機器人的抓取成功率及作業的整體效率。
現階段，針對剛性物體的位姿估計方法，大緻可以歸為以下三類：
1 基于模闆比對
2 基于3D描述符(描述子)
3 基于學習(神經網絡/ end to end)
基于模闆比對的方式
在可能的三維空間區域中，将待檢測物的三維模型進行不同視角下的渲染成像，提取各視角下物體的相關資訊(如：輪廓、邊緣梯度、表面曲率等)，對待檢測物體進行充分的采樣，提取足夠魯棒的模闆集。比對時，從場景中提取相關資訊，并在建立好的模闆集中進行查詢比對，找到相似度最高的模闆，并以該模闆的位姿作為粗位姿，然後結合ICP等方式進行位姿的細配準。
論文示例
《Real-Time 6D Object Pose Estimation on CPU》[1]
該方法以模闆比對為基本原理，從特征集模闆的建構、模闆的存儲資料結構以及記憶體重組優化等三個方面作出了改進和優化工作，以提高配準的準确度及整個流程的速度。
01 模闆特征集的建構
從多個視角對工件的CAD模型進行投影成像，并對每一個投影資料建構PCOF(Perspectively Cumulated Orientation Feature)特征集(每個視角對應一個模闆位姿)。
特征集在建構時，同時使用投影點雲的表面法向及輪廓梯度方向資訊建立方向直方圖(共8個方向)，并按照門檻值将直方圖進行二值化(組頻率大于門檻值的置為1)，最後用一個位元組(byte)來存儲(一個byte共8個bit位，對應8個方向)。
方向直方圖示例，ori(二值化的方向直方圖)，w(權重，以該直方圖中超過所設門檻值的最大頻率來表示)：
1,2兩像素點取自投影圖像的輪廓，以梯度方向建構直方圖
3,4兩像素點取自投影圖像表面，以法線方向建構方向直方圖
02 使用BPT資料結構存儲建構的模闆
在對模型進行投影成像時，視角點的選取是通過将正二十面體(下圖中depth 0)不斷等分，最終形成正1280面體(下圖中depth 3)，然後均勻選取正1280面體的頂點作為模型投影成像的視角點。
填充建構”樹”型存儲結構時，從第3層(depth 3)開始，實際的成像視角點也是從第三層選取，依次從上一層找到分割時的“父節點(頂點)”，并使用目前層中所有屬于該“父節點”的模闆位姿的均值作為“父節點”的位姿；
遞歸處理，直至第0層(depth 0)，以此建立一個4層的、由“粗”到“細”的模闆位姿樹(BPT(Balanced Pose Tree))
将建立的模闆按照“樹”型結構進行分層存儲(共四層)，查找時，實作由“粗”到“細”的模闆搜尋方式，以便加速模闆的比對查找，得到粗位姿。
03 利用SIMD技術實作記憶體重組
結合“樹”型資料存儲結構，利用SIMD(Single Instruction Multiple Data)技術實作記憶體重組，以達到加速的目的。
該方法示例效果：
該方法分别在桌面場景(左圖)
及無序抓取場景(右圖)下的測試效果
基于3D描述符(描述子)
三組對應點即可解析得到兩模型之間的對應位姿，是以，利用法向量、點間距等數學語言描述出模型表面的線條、邊緣等多種局部特征，進而找出準确的對應點。在得到比對的點對之後，再結合随機采樣一緻性(RANSAC)或者霍夫投票(Hough voting)等方式得到局部最優解，以此作為模型配準的粗位姿，最後利用ICP等方式進行精化得到最終結果。
論文示例
《Model Globally, Match Locally: Efficient and Robust 3D Object Recognition》[2]
該方法提出了一種新的局部描述符——PPF(Point Pair Featrue),通過對待比對工件的點雲中，任意兩個資料點之間建構PPF特征，最終形成一個該工件模型整體的PPF特征集。在實際比對時，從場景點雲中選取參考點，以該參考點為中心，與周圍的資料點建構PPF特征，并将該特征與模型特征集中的資料作比對，然後利用霍夫投票的形式，得到場景點雲中該參考點所屬工件的粗位姿，最後再利用ICP等方式進行位姿的細化配準。
01 構模組化型的PPF特征集(Model Globally)
整體模組化，建構對模型的全局描述，其中“Globally”是指對整個模型點雲進行特征(PPF，Point Pair Feature)的計算，然後将所有計算得到的特征集存儲在Hash表中，以供後續比對時查詢使用。(離線階段完成)
Model Globally 的本質是通過定義 Point Pair Feature，來建構特征矢量的集合以及每個特征矢量對應的“點對”集，作為 Global Model Desciption。
PPF 描述了兩個有向點(oriented points)的相對位置和姿态。假設有兩個點 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)
局部比對，從場景點雲中選取參考點，并與周圍資料點建構PPF特征，然後到hash表中查找是否存在一緻的特征值，由于是以場景的單個點對特征到模型的全局描述(hash table)中進行比對，是以稱為“局部”比對。
其中，恒定坐标系為整個比對流程的核心概念，由于模型及場景點雲中每個資料點都是法線已知，故将“模型點對”與“場景點對”各自旋轉，使其參考點的法向與恒定坐标系X軸已知，且參考點的位置與恒定坐标系原點重合，得到各自的變換矩陣(RT) Ts-g和Tm->g；當“場景點對”與“模型點對”同處于恒定坐标系下時，将“模型點對”繞X軸旋轉，使其與“場景點對”重合，得到旋轉角度值α，接着以該模型點的索引與α值，在累計投票表中進行投票。(參考圖示如下)
PPF(mr, mi) 為“模型點對”,mr為模型參考點
PPF(sr, si) 為“場景點對”，sr為場景參考點
PPF特征中的法線方向，皆為參考點的法線方向
建構的“場景點對”需要與有相同Hash索引的所有“模型點對”進行上述投票處理，故每一個“場景點對”都會生成一張投票表，其中，行數與模型點數相同，且與模型索引值一一對應；列數為劃分的角度數目，該方法中共劃分為30份，以(π/15)為角度間隔。(參考圖示如下)
從Hash表中搜尋與“場景點對”具有相同Hash索引的“模型點對”，然後進行累計投票
03 提取粗位姿
根據上述流程中統計得到的累計投分表尋找分值大于所設門檻值的位置。由于我們已經得到了“場景點對”及“模型點對”各自旋轉到恒定坐标系下的變換矩陣，以及“模型點對”繞X軸旋轉到“場景點對”的旋轉角度α，故可以計算出模型旋轉到場景中工件位姿的變換矩陣，即：
04 位姿聚類
每個參考點都可能會傳回多個位姿(分值相同或多個分值大于所設門檻值的位姿)，對所有傳回的位姿進行聚類，将位姿差異不超過所設門檻值的位姿歸為同一類，然後對每個聚類中的位姿取均值，依次代表該聚類的位姿，其分值為聚類中所有位姿的總分值，最後比較所有聚類的分值，将分值最高的聚類位姿作為配準得到的位姿輸出。
該方法示例效果：
在遮擋及噪聲場景下的測試
經物體識别與位姿估計後，模型以網格的形式展現，以作對比
基于學習(神經網絡)
這類方法也可以叫作基于回歸的方法。從模型的三維點雲資料或投影到多個視角下的的圖像資料中尋找位姿和圖像之間的内在聯系。常見的基于學習的方法可分為兩類，一類是直接回歸出物體的6D位姿(語義分割->Translation估計-> Rotation估計)；另一類方法是先估計3D KeyPoint在2D圖像上的投影點，然後使用PnP(Perspective-n-Point)等算法方式對位姿進行恢複。
論文示例
《PoseCNN: A convolutional neural network for 6d object pose estimation in cluttered scenes》[3]
在傳統的物體識别與位姿估計方式中，存在對于弱紋理、對稱以及有遮擋的物體的識别及位姿估計能力相對較弱的情況，針對這些問題，該方法提出一種端到端(end to end)的用于6D目标姿态估計的新型卷積神經網絡PoseCNN。PoseCNN通過在圖像中定位物體的中心并預測其與錄影機的距離來估計物體的三維平移。通過回歸到四元數(w,x,y,z)表示來估計物體的三維旋轉。其主要任務有三項：語義分割、3D平移估計、3D旋轉估計。此外，還引入了一種用于對稱物體姿态估計的新訓練損失函數ShapeMatch-Loss，并提供了一個大規模的RGB-D視訊資料集(YCB-Video dataset)，用于6D物體姿态估計。
網絡架構
該網絡共含有三個分支：語義分割分支、位置估計分支、姿态估計分支，每個分支都有一個loss函數，共三個。
01 位置估計分支
通過3D位置估計獲得平移矩陣T=(Tx,Ty,Tz)(目标物體在相機坐标系下的坐标)，一種簡單的估計T的方法是直接将圖像特征回歸到T，但是由于目标可能出現在圖像上的任意位置，是以這種方式是不可推廣的。并且不能處理同類的多個目标執行個體。該方法通過定位2D圖像的目标中心并估計目标到相機的距離來估計三維平移量。
(1)目标對象2D中心的預測(Hough Voting)
該層采用像素方式的語義标記結果和中心回歸結果作為輸入。對于每一個物體，首先計算圖像中的每個位置的投票得分，其分值表明了相應的圖像位置是物體的中心的可能性大小。物體的每個像素都會根據網絡預測添加圖像位置的投票，在處理完物體的所有像素後可以獲得所有圖像位置的投票分數，最後選擇最高分的位置作為物體中心。
通過如下公式，可以恢複得到位置平移矩陣中的Tx和Ty：
fx、fy表示相機焦距，(px，py)是主點，c是目标的二維中心
(2)深度預測Tz
生成了一系列目标中心之後，将投票給目标中心的像素視為該中心的初始值。然後，中心深度Tz的預測，被簡單地計算為由初始值預測的深度的平均值。
02 姿态估計分支
在Hough投票層可以預測得到的物體對象2D BBox，同時利用兩個RoI池化層對網絡第一階段生成的視覺特征進行剪裁和池化，然後進行3D旋轉回歸，得到目标對象的旋轉變換。
為了對四元數進行回歸訓練，該方法提出兩個損失函數：
1、PoseLoss(PLoss)，正确模型姿态上的點與相應使用估計方向上的點的平均平方誤差。
2、ShapeMatch-Loss(SLoss),不需要對稱規範，估計點與真值最近點的損失測量。
該方法示例效果：
分别在YCB-Video和OccludedLINEMOD資料集上作測試，
由上至下分别為：輸入資料、語義分割及中心點标注後的結果、
僅使用RGB資料、使用RGB-D資料并使用ICP進行位姿細化
參考文獻：
[1] Yoshinori Konishi, Kosuke Hattori, and Manabu Hashimoto. Real-time 6d object pose estimation on cpu. IEEE/RSJ International Conference on Intelligent 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.
更多機器視覺産品資訊與應用說明，機器視覺算法，應用技巧，解決方案以及相關領域的技術分享，歡迎繼續關注“漢振智能”....