1 引言
剛體的6D位姿估計，是指估計相機座標系下物體的6D位姿，即3D位置和3D姿態，此時物體本身的座標系可以看做是世界座標系，也即得到物體本身座標系到相機系的RT變換。剛體是指物體不會彎曲變形(手)，也不存在活動關節(胳膊)。剛體的6D位姿估計的意義在於能夠獲得物體的精確姿態，支撐對於物體的精細操作，主要應用於機器人抓取領域和增強現實領域。在機器人抓取領域，主流的方法是估計已知物體的6D位姿，進而獲得抓取器的目標6D抓取位姿。在增強現實領域，可以在物體上疊加虛擬元素，隨着物體的移動而保持和物體相對位姿不變。隨着SLAM等技術的成熟，機器人已經能夠在空間中進行很好的定位，但如果想要和環境中的物體進行交互，物體的6D位姿估計是必需的技術，也會持續成爲研究熱點。本文來自論文https://arxiv.org/abs/1905.06658，涉及的論文也都可以在論文中找到，也包含於 GitHub，轉載請註明出處，有錯誤請指正，歡迎大家多交流 ^ _ ^
2 回顧
剛體的6D位姿估計按照使用的輸入數據，可以分爲基於2D圖像的方法和基於3D點雲的方法。早期基於2D圖像的6D位姿估計方法處理的是紋理豐富的物體，通過提取顯著性特徵點，構建表徵性強的描述符獲得匹配點對，使用PnP方法恢復物體的6D位姿。對於弱紋理或者無紋理物體，可以使用基於模板的方法，檢索得到最相似的模板圖像對應的6D位姿，也可以通過基於機器學習的投票的方法，學習得到最優的位姿。
隨着2011年以kinect爲代表的的廉價深度傳感器的出現，在獲取RGB圖像的同時可以獲得2.5D的Depth圖像，進而可以輔助基於2D圖像的方法。爲了不受紋理影響，也可以只在3D空間操作，此時問題變成獲取的單視角點雲到已有完整物體點雲的part-to-whole配準問題。如果物體幾何細節豐富，可以提取顯著性3D特徵點，構建表徵性強的描述符獲得3D匹配點，使用最小二乘獲得初始位姿；也可以使用隨機採樣點一致算法(Ransac)獲得大量候選6D位姿，選擇誤差最小的位姿。
自2012年始，深度學習在2D視覺領域一騎絕塵，很自然的會將深度學習引入到物體6D位姿估計，而且是全方位的，無論是基於純RGB圖像、RGB和Depth圖像、還是隻基於3D點雲，無論是尋找對應、尋找模板匹配、亦或是進行投票，都展現了極好的性能。
隨着在實例級物體上的6D位姿估計趨於成熟，開始湧現了類別級物體6D位姿估計的方法，只要處理的物體在紋理和幾何結構上近似，就可以學習到針對這一類物體的6D位姿估計方法，這將極大提升這項技術在機器人抓取或者AR領域的實用性。
本文分別介紹基於2D圖像和基於3D點雲的，基於對應(Correspondence-based)、模板(Template-based)和投票(Voting-based method)的物體6D位姿估計方法，綜合如下表。
表1 剛體6D位姿估計方法綜述
3 基於對應的方法
這類方法的是指尋找輸入數據和已有物體的完整3D點雲之間的對應，如果輸入的是2DRGB圖像，可找到2D像素點和3D物體點之間的對應，進而使用PnP算法計算；如果輸入的是3D點雲，則可以使用3D特徵描述符尋找輸入點雲中3D點和已有完整物體3D點之間的對應，使用最小二乘法獲得6D位姿；兩類輸入都可以使用局部配準方法如ICP進行優化。
圖1 輸入2D圖像的基於對應的方法
基於2D圖像的方法主要針對紋理豐富的物體，3D模型首先投影到N個角度，得到N張模板RGB圖像，記錄3D點和2D像素之間的對應；採集單個視角下RGB圖像後，提取特徵點如SIFT，FAST，SURF，ORB等，尋找和模板圖像之間的對應(2D-2D)；這樣我們得到了3D點和當前觀測RGB圖像2D像素點的對應，使用Perspective-n-Point(PnP)算法即可恢復當前視角圖像的位姿(類似基於特徵點vSLAM中的重定位過程)。除了傳統的特徵描述子，也出現了基於深度學習的特徵描述子例如Lift，Glampoints等。
除了顯式尋找特徵點之間的對應，也出現了很多基於深度學習的方法，隱式地預測3D點在2D圖像上的投影，進而使用PnP算法估計6D位姿。物體上特徵點的選擇不是那麼直接，因此很多方法預測物體最小3D包圍盒的8個頂點在2D圖像上的投影，例如BB8[2017-Bb8]和Yolo-6D[2018-Real-time]。一些方法構建局部3D控制點，預測3D控制點在2D圖像上的投影點，例如 [2018-Robust 3d]。包圍盒的投影點有可能落在圖像外，一些其他方法預測2D圖像上的物體區域對應的所有3D點，通常藉助物體座標系(3D空間每一個點對應一個顏色值)，例如[2018-Segmentation-driven]，Dpod[2019-Dpod]，[2019-Single-stage 6d]。
圖2 輸入3D點雲的基於對應的方法
基於3D點雲的方法主要基於3D特徵描述符尋找兩片點雲之間的對應。常用的3D局部特徵描述符，如Spin Images，3D Shape Context，FPFH，CVFH，SHOT等都可以使用，一些基於深度學習的3D描述符例如3DMatch，3DFeat-Net和StickyPillars等也可以使用。
4 基於模板的方法
這類方法是從標記好6D位姿的模板中，選擇最相似的模板，將其6D位姿作爲當前物體的6D位姿。這類方法針對的是弱紋理或者無紋理圖像，也即Correspondence-based methods很難處理的情況；在2D情況下，模板通常爲已知3D模型在不同角度下的投影圖像，每個投影圖像都對應一個物體的6D位姿，此時問題變成了一個圖像檢索的問題。在3D情況下，模板是指目標物體的完整點雲，需要尋找到，能夠使輸入的單視角點雲對齊到完整點雲的最佳6D位姿，此時問題轉變爲一個部分配準問題。
圖3 輸入2D圖像的基於模板的方法
基於2D圖像方法的代表方法是LineMode方法[2012-Model based]，通過比較觀測RGB圖像和模板RGB圖像的梯度信息，尋找到最相似模板圖像，以該模板對應的位姿作爲觀測圖像對應的位姿，該方法還可以結合深度圖的法向量來提高精度；[2015-Detection and fine]也利用了模板匹配策略。除了顯式尋找最相似的模板圖像外，也有方法隱式地尋找最近似的模板，代表性方法是AAE[2018-Implicit 3d]。該方法中，上萬模板圖像通過編碼形成碼書，輸入圖像變成一個編碼和碼書進行比較尋找到最近似的模板。
一些方法直接從圖像中恢復目標物體的6D位姿，這個過程可以看作從已訓練的帶標籤的圖像中，尋找和當前輸入圖像最近似的圖像，並且輸出其對應的6D位姿標籤。這類方法直接尋找輸入圖像到位姿參數空間的映射，而且較易和目標檢測框架結合。這類方法較多，代表性的有PoseCNN[2017-Posecnn]，SSD6D[2017-Ssd-6d]，Deep-6DPose[2018-Deep-6dpose]，[2019-Recovering 6d]，[2019-Cdpn]，[2020-Robust 6d]等。另外一類方法能夠針對一類物體構建隱式表示，這類方法也可以看作是基於模板的方法，代表性的有NOCS[2019-Normalized]，[2019-Latentfusion]，[2020-Learning canonical]等。
圖4 輸入3D點雲的基於模板的方法
基於3D點雲的方法主要爲多種全局配準方法，如Super 4PCS[2014-Super 4PCS]和GO-ICP[2015-Go-icp]等，能夠得到相對準確的6D位姿，結果可以使用ICP進行優化。一些深度學習進行兩片點雲配準的方法也湧現出來，包括PCRNet [2019-Pcrnet], DCP [2019-Deep closest point], PointNetLK [2019-Pointnetlk]，[2019-One framework], TEASER [2020-Teaser]等。在進行配準時，可以將多個視角拼合使輸入數據更完整，也可以將完整物體投影多個角度得到多個單片點雲，從而輔助配準。也有一些方法，不需要輸入兩片點雲，給定單個視角點雲，能夠直接回歸6D位姿，例如[2020-6d object pose]。
5 基於投票的方法
這類方法是指每個像素或者每個3D點通過投票，貢獻於最終的6D位姿估計。依據是圖像中的每一個局部都能夠對整體的輸出產生投票，對遮擋情況較爲有效。基於對應的方法主要使用特徵點，基於模板的方法主要使用整體，基於投票的方法每個基元都進行貢獻。存在兩類投票策略，分別爲間接投票和直接投票。間接投票是指每個像素或者3D點投票得到預定義的特徵點，能夠得到2D-3D或者3D-3D的對應。直接投票是指每個像素或3D點直接投票得到一個確定的6D物體座標系或者6D位姿。
圖5 基於間接投票的方法
間接投票方法中，2D輸入的代表性方法爲PVNet[2018-PVNet]，投票得到一系列3D點在2D圖像上的投影點像素座標。3D輸入情況下，代表性方法有PVN3D[2019-PVN3D]，6-PACK[2019-6-pack]，YOLOff[2020-Yoloff]等，直接基於3D深度神經網絡，投票得到3D特徵點，進而使用最小二乘法得到相對位姿。其中6-PACK能夠處理類別級物體的6D位姿估計。
圖6 基於直接投票的方法
直接投票方法中，通過生成大量位姿預測，再進行選擇和優化，可以得到最終的位姿。2D輸入的代表性方法有[2014-Learning 6d object pose]和[2014-Latent-class]，3D輸入的代表性方法有PPF[2012-3d object detection]，[2018-6d pose estimation]。基於深度學習的有DenseFusion[2019-Densefusion]，分別對RGB圖像和Depth對應3D點雲使用3DCNN網絡得到融合的pixel-wise dense feature，每一個feature都能預測一個姿態，最後通過投票得到最後的6D姿態。此外還有[2020-Lrf-net]，[2020-6dof object pose]等。
6 方法對比與總結
基於點雲配準的方法可以參照一些綜述，這裏主要比較基於RGB-D的方法，也即在數據集LineMode[2012-Model based]，Occlusion Linemod[2014-Learning 6d object pose]和YCB-Video[2015-The YCB]數據集上進行評測的方法。評測度量主要有針對非對稱物體的ADD度量，和針對對稱物體的ADD-S度量。ADD大意是指按照估計RT和真值RT分別作用於點雲後，所有點的平均距離偏差。ADD-S考慮到對稱物體，尋找變換後，每一點到另一片點雲的最近鄰點，計算他們的平均距離偏差。評測時對LineMod數據集中的非對稱物體就用ADD度量，對稱物體就用ADD-S度量，如果度量結果小於模型直徑的10%，則認爲位姿估計正確。對於YCB-Video數據集，經常用的度量是ADD-S度量，而且閾值經常設置爲2cm(面向抓取應用)，而且ADD-S曲線下的面積(AUC)也被使用，其閾值設爲10cm。
在LineMode和Occlusion Linemod，YCB-Video上的對比結果如下面兩個表所示，可以看到當前方法都得到了非常高的精度，已經能夠滿足實例級別物體6D位姿估計的需要，具體採用哪一種方法可以依據當前可獲得的數據，需要的算力，要求的精度等等來選擇。常用的構建自己數據集的方法有LabelFusion[2018-Label Fusion]。後續方向會逐步擴展到泛化能力，也即類別級物體的6D位姿估計。
參考文獻
2012-Model based training, detection and pose estimation of texture-less 3d objects in heavily cluttered scenes
2012-3d object detection and localization using multimodal point pair features
2014-Learning 6d object pose estimation using 3d object coordinate
2014-Latent-class hough forests for 3d object detection and pose estimation
2014-Super 4PCS: Fast Global Pointcloud Registration via Smart Indexing
2015-Detection and fine 3d pose estimation of texture-less objects in rgb-d images
2015-Go-icp: A globally optimal solution to 3d icp point-set registration
2015-The YCB object and model set: Towards common benchmarks for manipulation research
2017-Bb8: a scalable, accurate, robust to partial occlusion method for predicting the 3d poses of challenging objects without using depth
2017-Posecnn: A convolutional neural network for 6d object pose estimation in cluttered scenes
2017-Ssd-6d: Making rgb-based 3d detection and 6d pose estimation great again
2018-6d pose estimation using an improved method based on point pair features
2018-Deep-6dpose: recovering 6d object pose fromasinglergbimage
2018-Implicit 3d orientation learning for 6d object detection from rgb images
2018-Label Fusion: A Pipeline for Generating Ground Truth Labels for Real RGBD Data of Cluttered Scenes
2018-Learning to predict dense correspondences for 6d pose estimation
2018-PVNet Pixel-wise Voting Network for 6DoF Pose Estimation
2018-Real-time seamless single shot 6d object pose prediction
2018-Robust 3d object tracking from monocular images using stable parts
2018-Segmentation-driven 6d object pose estimation
2019-6-pack: Category-level 6d pose tracker with anchor-based keypoints
2019-Cdpn: Coordinates-based disentangled pose network for realtime rgb-based 6-dof object pose estimation
2019-Deep closest point: Learning representations for point cloud registration
2019-Densefusion: 6d object pose estimation by iterative dense fusion
2019-Dpod: 6d pose object detector and refiner
2019-Latentfusion: End-to-end differentiable reconstruction and rendering for unseen object pose estimation
2019-Normalized object coordinate space for category-level 6d object pose and size estimation
2019-One framework to register them all: Pointnet encoding for point cloud alignment
2019-Pcrnet: Point cloud registration network using pointnet encoding
2019-Pointnetlk: Robust & efficient point cloud registration using pointnet
2019-PVN3D: A Deep Point-wise 3D Keypoints Voting Network for 6DoF Pose Estimation
2019-Recovering 6d object pose from rgb indoor image based on two-stage detection network with multi-task loss
2019-Single-stage 6d object pose estimation
2020-6d object pose regression via supervised learning on point clouds
2020-6dof object pose estimation via differentiable proxy voting loss
2020-Learning canonical shape space for category-level 6d object pose and size estimation
2020-Lrf-net: Learning local reference frames for 3d local shape description and matching
2020-Robust 6d object pose estimation by learning rgb-d features
2020-Teaser: Fast and certifiable point cloud registration
2020-Yoloff: You only learn offsets for robust 6dof object pose estimation