6d姿態(tài)估計(jì)是什么 6D位姿估計(jì)PPF算法詳解

一、論文解讀

論文：?Drost et al. Model Globally, Match Locally: Efficient and Robust 3D Object Recognition. CVPR, 2010. http://campar.in.tum.de/pub/drost2010CVPR/drost2010CVPR.pdf Model Globally, Match Locally?論文名字用 4 個(gè)詞高度總結(jié)了 PPF 算法的精髓：“整體建模，局部匹配”。下面研讀一下摘要（Abstarct）：

[1] This paper addresses the problem of?recognizing free form 3D objects in point clouds.

第一句話表明論文的研究方向與問(wèn)題：3D物體點(diǎn)云（free form）的識(shí)別

[2] Compared to traditional approaches based on?point descriptors, which depend on local information around points, we propose a novel method that?creates a global model description based on oriented point pair features and matches that model locally using a fast voting scheme.

第二句話既表明論文的創(chuàng)新點(diǎn)（與前人方法的區(qū)別），又用一句話總結(jié)本文的中心思想。 ·Model Globally:?creates a global model description based on oriented point pair features ·Match Locally:?matches that model locally using a fast voting scheme

[3] The?global model description?consists of all model point pair features and represents a mapping from the point pair feature space to the model, where similar features on the model are grouped together.

第三句話緊接著解釋了什么是 global model description

[4] Such representation allows using much sparser object and scene point clouds, resulting in very fast performance.

第四句話說(shuō)明這種 representation 的優(yōu)點(diǎn)：稀疏采樣，提升算法速度

[5] Recognition is done locally using an efficient voting scheme on a reduced two-dimensional search space.

第五句話表明如何實(shí)現(xiàn) recognition：使用有效的投票機(jī)制在二維搜索空間里做局部匹配

[6] We demonstrate the efficiency of our approach and show its high recognition performance in the case of?noise, clutter and partial occlusion.

第六句話表明本文算法針對(duì)的場(chǎng)景問(wèn)題：干擾、堆疊、部分遮擋

[7] Compared to state of the art approaches we achieve better recognition rates, and demonstrate that with a slight or even no sacrifice of the recognition performance our method is much faster then the current state of the art approaches.

第七句話說(shuō)明通過(guò)與 SOTA 方法的對(duì)比，文中提出的方法既保證了高的識(shí)別率，又在不犧牲識(shí)別性能的情況下提升了算法的速度。 接下來(lái)看一下論文的算法：

1. Model Globally

首先定義兩個(gè)詞：scene?和?model?，scene 是我們測(cè)得的真實(shí)場(chǎng)景（點(diǎn)云），model 是物體的真實(shí)模型（點(diǎn)云）。

Both the scene and the model are represented as a finite set of oriented points, where a normal is associated with each point.

符號(hào)表示：

?points in the scene

?points in the model

（1）Point Pair Feature (PPF) Model Gobally 的本質(zhì)是通過(guò)定義 Point Pair Feature，來(lái)構(gòu)建特征矢量的集合以及每個(gè)特征矢量對(duì)應(yīng)的點(diǎn)對(duì)集，作為 Global Model Desciption。所以，首先先定義 PPF。 PPF 描述了兩個(gè)有向點(diǎn)（oriented points）的相對(duì)位置和姿態(tài)。 假設(shè)有兩個(gè)點(diǎn)??和??，法向量（normals）分別為??和??，??，則 PPF 定義為： 注：??為兩個(gè)矢量的夾角，且??是非對(duì)稱的。PPF 示意圖見(jiàn) Figure2.(a)。 有了 Point Pair Feature，就可以用其來(lái)定義 Global Model Description。 （2）Global Model Description 注：Global Model Description 是離線（in the off-line phase）構(gòu)建的 先看原文：

The model is represented by a set of?point pair features?with similar?feature vectors?being grouped together.

實(shí)現(xiàn)方法： 第一步：計(jì)算 model 表面所有 point pairs 的特征矢量??，其中 distances 和 angles 分別以??和??的步長(zhǎng)做采樣； 第二步：構(gòu)建哈希表（hash table），將具有相同 feature vector??的 point pair 放在一起，即哈希表的鍵（key ）為 feature vector??，值（value）為具有相同特征矢量的點(diǎn)對(duì)集??，如 Figure2.(b) 所示。 ?

The global model description is a mapping from the sampled point pair feature space to the model.

2. Match Locally

當(dāng)定義好全局模型描述（Global Model Description）后，就可以考慮局部匹配了。 All model features??that are similar to a given scene feature??can then be searched in constant time by using??as a key to access the hash table. （1）Local Coordinates 局部匹配的大概思路如下： ·從 scene 中選取任意一個(gè)參考點(diǎn)??，假設(shè)它在物體的表面上，若假設(shè)正確，則在 model 存在一個(gè)點(diǎn)??與??對(duì)應(yīng)； ·將這兩個(gè)參考點(diǎn)配準(zhǔn)，需同時(shí)將點(diǎn)的位置和法向量對(duì)齊； ·讓 model 繞??的法向軸轉(zhuǎn)動(dòng)一定角度與 scene 配準(zhǔn) 由此看來(lái)，從 model space 到 scene space 的剛體變換可以由 model 中的一點(diǎn)和轉(zhuǎn)動(dòng)角度??來(lái)描述，將這個(gè) pair??定義為 model 相對(duì)于參考點(diǎn)??的Local Coordinates 。 現(xiàn)在明確一下思路： ·給定參考點(diǎn)??，選取與 scene 點(diǎn)對(duì)??具有相似 fecture vector?（same distance and relative orientation）的 model 點(diǎn)對(duì)??； ·通過(guò)變換矩陣??將??移動(dòng)到 Local Coordinates 的原點(diǎn)，并且轉(zhuǎn)動(dòng) model，使其法向軸??與 Local Coordinates 的??軸重合； ·同理，通過(guò)??對(duì) scene 做相同操作； ·最后，將 model 中的一點(diǎn)??繞??軸轉(zhuǎn)動(dòng)??與??配準(zhǔn) 通過(guò)上述描述，可以將從 model 到 scene 的 transformation 定義為：??，如 Figure 3 所示。

（2）Voting Scheme 現(xiàn)在我們可以將研究問(wèn)題定義如下： ? 前面我們定義了 local coordinates，現(xiàn)在只需要通過(guò)一種方法找到最優(yōu)的 local coordinates 使得 scene 中落在 model 表面的點(diǎn)最多，即可求出物體 pose。 論文通過(guò)投票機(jī)制實(shí)現(xiàn)，定義一個(gè)二維的 accumulator array，行（rows）數(shù)??為 model 采樣點(diǎn)??的個(gè)數(shù)，列（columns）數(shù)??為按采樣步長(zhǎng)??的旋轉(zhuǎn)角??的個(gè)數(shù)。 This accumulator array represents the discrete space of local coordinates for a fixed reference point. 具體實(shí)現(xiàn)： ·對(duì)于 scene 的參考點(diǎn)??，與 scene 中所有其他的點(diǎn)組成點(diǎn)對(duì)??，對(duì)每一個(gè)點(diǎn)對(duì)，計(jì)算??； ·將??作為 key，搜索 global model description 的哈希表，找到與??類似（distance & normal）的 model 特征矢量??和點(diǎn)對(duì)??； ·對(duì)于每個(gè)匹配的 model 點(diǎn)對(duì)??，通過(guò)之前的公式??可以算出旋轉(zhuǎn)角??； ·對(duì)??與二維數(shù)組中離散??對(duì)應(yīng)的位置投票（+1）； ·全部計(jì)算完后，我們就可以得到參考點(diǎn)??的最大得票所對(duì)應(yīng)的??和??，即最優(yōu)的 local coordinates?

（3）Efficient Voting Loop 對(duì)每個(gè)點(diǎn)對(duì)都要求解??，為了加速計(jì)算，將??分解：??，這樣就可以分別計(jì)算??和??了。 根據(jù)??和??，求得： 注： ·對(duì)于 model 或者 scene 中的每個(gè)點(diǎn)對(duì)，??都是唯一的 ·對(duì)于 model 中的每個(gè)點(diǎn)對(duì)，??可以在離線階段求解 ·對(duì)于 scene 中的每個(gè)點(diǎn)對(duì)，??只需要算一次 （4）Pose Clustering 之前的算法基于我們的假設(shè)：參考點(diǎn)是在物體表面的。因此，我們需要在 scene 點(diǎn)云中采樣多個(gè)參考點(diǎn)??，保證至少有一個(gè)參考點(diǎn)能在物體表面。 每個(gè)參考點(diǎn)可能返回多個(gè)位姿（投票相同），返回的位姿（retrieved poses）是否逼近 ground truth，取決于 model 和 scene 點(diǎn)的采樣率和旋轉(zhuǎn)角的采樣。 我們對(duì)所有返回的位姿做聚類，每個(gè) clutter 中位姿的位置、姿態(tài)的差異不超過(guò)設(shè)定的閾值，然后每個(gè)聚類的得分是其包含的所有位姿的總得分，找出得分最高的 clutter，則最終的位姿 為得分最高的 clutter 里面包含位姿的平均值。 如果場(chǎng)景中存在物體的多個(gè)實(shí)例，則會(huì)返回多個(gè) clutters。

二、 OpenCV 實(shí)現(xiàn)

opencv_contrib 代碼： https://github.com/opencv/opencv_contrib/tree/master/modules/surface_matching 文檔 Documentation： https://docs.opencv.org/3.0-beta/modules/surface_matching/doc/surface_matching.html 測(cè)試數(shù)據(jù)集： http://staffhome.ecm.uwa.edu.au/~00053650/recognition.html 注意： ·Mian數(shù)據(jù)集 ply 文件不含 normal 信息，需先計(jì)算法向量 ·可視化使用的 open3d（顏色渲染：z coordinate as color） 測(cè)試結(jié)果：

model scene 將經(jīng)過(guò)位姿轉(zhuǎn)換的 model 點(diǎn)云與 scene 點(diǎn)云疊加的效果： transformed model & scene

三、Matlab 實(shí)現(xiàn)

Github Project: https://github.com/guglu/ppf-matching 注： 代碼中 mex 可執(zhí)行文件應(yīng)該是在 windows 平臺(tái)下編譯的，而我是在 ubuntu 系統(tǒng)中使用 matlab，所以需要重新編譯一下，在 command line 中運(yùn)行：

mex mex/computePPFmex.cpp mex mex/computePPFmex.cpp mex mex/MurmurHash3.cpp 運(yùn)行 test_detector.m 腳本即可。 ? 結(jié)果：??

?

四、PCL 實(shí)現(xiàn)

PCL 中關(guān)于 PPF 的示例代碼如下： https://github.com/PointCloudLibrary/pcl/blob/master/apps/src/ppf_object_recognition.cpp PCL 中定義了： PPFSignature：ppf 特征，定義在 point_types.hpp 中，是一種數(shù)據(jù)類型； PPFHashMapSearch：哈希表搜索模板類 PPFRegistration：ppf 配準(zhǔn)模板類 建議在 PPF 之后用 ICP 迭代優(yōu)化。

編輯：黃飛