参考资料

1. 论文: 3D Object Detection and Localization using Multimodal Point Pair Features 2012
2. 论文: Model Globally, Match Locally: Efficient and Robust 3D Object Recognition 2010
3. 论文：A refined icp algorithm for robust 3-d correspondence estimation 2003
4. 论文：Efficient Variants of the ICP Algorithm 2001
5. PPF3DDetector API
6. ICP API

---

3D面匹配算法简介

1. 参考资料

1. 论文: 3D Object Detection and Localization using Multimodal Point Pair Features 2012
2. 论文: Model Globally, Match Locally: Efficient and Robust 3D Object Recognition 2010
3. PPF3DDetector API

2. 问题定义

输入 :

​ (1) 要检测目标的3D模型数据$M$ 。可以通过CAD或者从多张RGBD图像中重建。每个数据项应包含3D点的坐标以及其法向量。

​ (2) 要检测的场景的3D数据。通过RGBD传感器获得，即我们有目标场景的RGB(灰度)图$I_C$和深度图$I_R$ 。分别由域 ${\Omega }_C$ 和 ${\Omega }_R$ 定义。

目标 :

​ 当场景中存在目标时，准确定位到目标的位置并求得在场景中目标相对于相机的姿态$p$ 。

方法 :

​ 根据目标的3D模型数据$M$ ，训练一个目标的模型–>在场景中检测3D特征点–>匹配特征点，求得姿态$p_0$ 的一个初始估计–>利用ICP(iterative closest point)算法，将$p_0$作为初始输入，对求得的姿态进行进一步的精炼(refine)操作以得到一个更准确的结果$p$ -->输出最后的结果$p$ 。

3. 多模态特征(Multimodal Feature)

参考资料：

​ 论文: 3D Object Detection and Localization using Multimodal Point Pair Features 2012

参考论文[1]将边缘点分为两类。第一类是纹理边缘(texture edge)，第二类是几何边缘(geometric edge)。 所谓的纹理边缘即场景内由于目标内部的纹理所产生的边缘，相对的，几何边缘就是场景内由目标到背景的突变所产生的边缘。作者论文中提到：对于3D匹配而言，几何边缘点是一种比较好的特征点，而纹理边缘点对于匹配并没有多少用处，相反反而会带来更大的计算量。因此论文中提到了一种高效的几何边缘点提取方法。所谓的多模态特征，便是基于几何边缘点 $e$ 和参考点(reference point) $r$ 进行定义和计算的。其中参考点 $r$ 通过对模型进行均匀采样获得。
多模态特征对于尺度(scale)变化、沿着视点方向的旋转(rotations of the object around the viewing direction)、以及透视畸变(perspective distortions)具有不变性。因而对于不同的视点，只需要一个模板图片就够了。下面是计算多模态特征的步骤

(1) 投影矫正(Perspective Correction)

示意图如下：

将边缘点$\mathbf{e}\in {\mathbf{\Omega }}_{\mathbf{E}}$ 和参考点 $\mathbf{r}\in {\mathbf{\Omega }}_{\mathbf{R}}$ 及其法向量 ${\mathbf{n}}_{\mathbf{r}}$ 重投影到一个新的平面 ${\mathbf{I}}^{{}^{\prime }}$ 上($\mathbf{e}$ 是从 ${\mathbf{I}}_C$ 上选取的点， $\mathbf{r}$ 是从 ${\mathbf{I}}_R$ 上选取的点)。新的投影平面 ${\mathbf{I}}^{{}^{\prime }}$ 定义为垂直于向量 ${\mathbf{v}}_{\mathbf{r}}=\frac{\mathbf{r}}{|\mathbf{r}|}$ 且焦距与原投影平面具有相同的焦距的平面。重投影之后，参考点 $\mathbf{r}$ 位于新的投影平面的中心。通过重投影这一预处理，使得多模态特征具有上面提到的：沿着视点方向的旋转(rotations of the object around the viewing direction)不变形，以及透视畸变(perspective distortions)不变性。后文中出现的 $\mathbf{r}$ 和 $\mathbf{e}$ 都是重投影之后的点。

(2) 几何边缘检测(Geometric Edge Detection)

几何边缘点检测基于两步进行：
a. 在RGB图 $I_C$ 中利用Canny边缘算子进行边缘检测，求得所有几何边缘的候选点。
b. 对第一步得到的所有候选点，计算其在深度图 $I_R$ 中沿着该点边缘梯度方向的一条线段上的最大和最小深度值。如果两者深度差值大于一定阈值，则认为该点为几何边缘点，否则为纹理边缘点。(论文中给出深度阈值一般1-3即可)

经过这一步操作后，我们已经得到了场景中经过重投影后的所有几何边缘点 $\mathbf{e}$ 、参考点 $\mathbf{r}$ 、$\mathbf{r}$的法线方向 ${\mathbf{n}}_r$ 以及 $\mathbf{r}$ 的单位向量 ${\mathbf{v}}_r=\frac{\mathbf{r}}{|\mathbf{r}|}$ ，下面开始进入多模态特征的计算。(注意，对于所有边缘梯度向外的边缘点，我们将其边缘梯度方向进行重定向，使得所有的边缘梯度方向都是指向物体内部的)

(3) 多模态特征的计算(Calculate Multimodal Feature)

多模态特征是一种4维的点对特征(point pair feature)，定义如下：
$$F(\mathbf{e},\mathbf{r})=(d(\mathbf{e},\mathbf{r}),{\alpha }_d,{\alpha }_n,{\alpha }_v)\text{\hspace{1em}(1)}$$
其中每个特征维度定义如下：
$$d(\mathbf{e},\mathbf{r})=Z(\mathbf{r})\frac{|\mathbf{e}-\mathbf{r}|}{f}\text{\hspace{1em}(2)}$$
其中 $f $ 为相机焦距，$Z(\mathbf{r})$ 为参考点 $\mathbf{r}$ 的深度。这样的定义，使得 $F(\mathbf{e},\mathbf{r})$ 具有缩放不变形。
$${\alpha }_d=\angle ({\mathbf{e}}_d,\mathbf{e}-\mathbf{r})\text{\hspace{1em}(3)}$$
其中 ${\mathbf{e}}_d$ 为边缘的梯度方向。
$${\alpha }_n=\angle ({\mathbf{n}}_r,\mathbf{e}-\mathbf{r})\text{\hspace{1em}(4)}$$

$${\alpha }_v=\angle ({\mathbf{n}}_r,{\mathbf{v}}_r)\text{\hspace{1em}(5)}$$

示意图如下：

4. 模型描述(Model Description)

参考资料：

​ 论文: Model Globally, Match Locally: Efficient and Robust 3D Object Recognition 2010

这一步，作者参考了上面参考论文中 3.2 Global Model Description 小节的内容，但是特征描述符使用作者自己提出的PPF多模态特征描述符。在离线阶段，我们需要对目标模型在不同视点下显示出来的可见部分的表面进行一个描述。这种描述通过一个hash表来完成。该hash表将每一个量化(离散化)后的多模态特征向量映射到一个具有相似特征向量的特征点对列表中。示意图如下：

其中左边为模型表面上具有相近特征向量的点对，右边为对应的hash表，这些点对被放在同一个hash slot中。

5. 投票方法(Voting Scheme)

参考资料：

​ 论文: Model Globally, Match Locally: Efficient and Robust 3D Object Recognition 2010

论文使用一种类似于广义霍夫变换(GHT)的方法，对所有可能的结果进行投票，得到所有满足条件的结果。再对这些结果进行聚类操作得到最好的结果。最后，利用ICP算法对结果进行Refinement操作。

(1) Voting

首先说一下参考论文中提到的一个局部坐标系$({\mathbf{m}}_i,\alpha )$ :

如上图所示。其定义为:
$${\mathbf{s}}_i=T_{s\to g}^{-1}{R}_x(\alpha )T_{m\to g}{\mathbf{m}}_i\text{\hspace{1em}(6)}$$
其中 $T_{m\to g}$ 将 ${\mathbf{m}}_i$ 变换到以原点为起点且法向量 ${\mathbf{n}}_m$ 被旋转到 $x$ 轴上，$T_{s\to g}$ 对 ${\mathbf{s}}_i$ 执行同样的操作。这个局部坐标系的意义在于：对于场景中的每一个参考点(${\mathbf{s}}_r\in S$)，确定一个位于模型上的与参考点 ${\mathbf{s}}_r$ 满足最佳匹配的条件的点 ${\mathbf{m}}_i$ 以及它们之间的一个沿 $x$ 轴的旋转角度 $\alpha$ ，那么场景中的模型的姿态便能由 $({\mathbf{m}}_i,\alpha )$ 唯一确定。因此，对每一个参考点 ${\mathbf{s}}_r$ 而言，它的投票空间即是以所有可能角度 $\alpha$ 为横轴，以模型上的所有点 ${\mathbf{m}}_i$ 为纵轴的一个2D平面。

投票过程便是对每一个参考点 ${\mathbf{s}}_r$ ，计算它与场景中每一个几何边缘点 ${\mathbf{e}}_i$ 的PPF特征 $\mathbf{F}({\mathbf{s}}_r,{\mathbf{e}}_i)$ ，再通过模型描述中建立的hash表，找到所有可能的模型上的对应点对 $({\mathbf{m}}_i,{\mathbf{m}}_j)$ ，计算对应的参数 ${\alpha }_i$ 并在投票空间中点 $({\mathbf{m}}_i,{\alpha }_i)$ 上进行累加。所有点对 $({\mathbf{s}}_r,{\mathbf{e}}_i)$ 投票完成后，找到投票空间中所有满足阈值条件的点 $({\mathbf{m}}_i,{\alpha }_i)$ ，作为下一步操作的输入。投票过程示意图如下：

(2) Pose Clustering

投票完成后，对于每一个参考点 ${\mathbf{s}}_r$ 我们都到了一组满足条件的姿态 $p_i$ 。对于所有的姿态 $P$ ，通过聚类将之分成多个组。对于每一个，计算组内所有姿态的分数加权和作为该组的一个评分。每个姿态的分数即为该姿态在投票环节所得的票数。选取分数最高的组的所有姿态的均值作为最终的结果。

(3) Pose Refinement:

仅仅通过上述步骤得到结果，通常还具有一定的误差，一般旋转角度误差在 $10^{。}$ 内，平移距离误差在模型直径的 0.005 内都算正常范围。因此，对于聚类得到的结果，还需要利用ICP算法进行Refinement操作，从而得到最终的最佳匹配结果。ICP算法细节见下一节。

---

ICP算法

1. 参考资料

1. 论文：A refined icp algorithm for robust 3-d correspondence estimation 2003
​ 2. 论文：Efficient Variants of the ICP Algorithm 2001
3. ICP API

2. 问题定义

输入：

​ (1) 场景的3D点的坐标集合 $A$

​ (2) 模型的3D点的坐标集合 $B$

目标：

​ 求解一个点对集合 C = { ( i , j ) ∣ a i ∈ A 　 a n d 　 b j ∈ B } C = \{(i, j) | \boldsymbol a_i \in A　and　\boldsymbol b_j \in B\} C={ (i,j)∣ai​∈A　and　bj​∈B} ，或者使得两个点集间对应点对某种误差最小的运动模型的参数 $\mathbf{R},\mathbf{t}$ 。

方法：

​ Picky ICP 算法。

3. 算法流程

Picky ICP 算法不同于其他ICP算法，该算法采用分层的思想，每次只对输入点集 $A$ 中的一部分点进行迭代计算，当算法收敛时，再对下一层的点进行同样的计算，并将当前计算结果作为下次计算的初始值。

(1) 选取控制点(selection of control point)

对输入场景点集，将其分为 $h+1$ 层。第一次选取下标为 $2^h$ 的倍数的点作为控制点。此后逐层选取下标为 $2^{h-1},2^{h-2},...,2^0$ 的点作为控制点，直到所有点都被选为控制点。

(2) 计算对应点对(computation of point pair)

对每一个控制点，计算模型点集 $B$ 中的最近点，作为其对应点。可以采用k-d树进行加速。同时，对于存在多个对应点对的情况，只保留距离最近的点对。

(3) 删除离群点(reject outliers)

对第二步得到的所有点对，通过点对距离阈值判断的方法，删去一些离群点对，增强算法的鲁棒性。

首先计算所有点对的标准差 $\mathbf{\sigma }$ ，当 $dist({\mathbf{a}}_i,{\mathbf{b}}_j)>SCALE\ast \mathbf{\sigma }$ 时，该点对被认为是离群值，在后续的计算中，该点对被忽略。

(4) 迭代计算模型参数(computation of motion)

利用误差平法和作为误差度量：
$$({\mathbf{R}}^{{}^{\prime }},{\mathbf{t}}^{{}^{\prime }})={\mathrm{argmin}}_{(\mathbf{R},\mathbf{t})}\sum _{(i,j)\in C}||{\mathbf{b}}_j-\mathbf{R}{\mathbf{a}}_i-\mathbf{t}|{|}^2\text{\hspace{1em}(7)}$$
求解使得该误差最小的 $(\mathbf{R},\mathbf{t})$ 。可以利用迭代的方法进行求解。

(5) 迭代停止条件

当 $(\mathbf{R},\mathbf{t})$ 的参数变化小于某个阈值或者迭代次数达到最大迭代次数时，停止计算。

---

运行效果截图

测试平台配置：

​ 内存：8G

​ 处理器：Intel Core i5-7500 3.40GHz x 4

​ 操作系统：Ubuntu 64-bit

输入模型点集大小： 28291

输入场景点集大小：114373

测试结果：

Training costs: 329.72 s
Matching costs: 2.10481 s
Number of matching poses: 20
Performing ICP on 2 poses…
ICP costs: 1.57771 s

截图：

其中绿色点云为模型，红色点云为场景，蓝色点云为使用Pose result 0 将模型变换到场景中得到的结果。

---

重要API

1. 参考资料

(1) OpenCV PPF3DDetector API

(2) OpenCV ICP API

(3) OpenCV Compute Normals API

2. PPF3DDetector API

(1) 创建PPF特征检测器

/**
 * @brief: 创建3D PPF 特征检测器，并指定相关参数
 * @param relativeSamplingStep: 相对于模型直径的采样步长。在对模型进行模型描述建模时，用于
 在模型上采样的步长。值越小，模型越稠密，姿态估计越准确，但是内存要求以及训练时间越长。值越大，
 模型越稀疏，姿态估计精度降低，但是内存要求以及训练时间和匹配时间更短。
 * @param relativeDistanceStep: 相对于模型直径的离散步长。在对模型进行模型描述建模时，用
 于对PPF特征向量进行离散化的步长。值越小，则离散化越精细，哈希表越大，但是哈希表每个bin之间的
 关系越模糊。值越大，细化越粗糙，哈希表越小，但是两个不同PPF特征向量可能会因为过大的步长而被放
 入相同的哈希槽中。默认该值与 'relativeSamplingStep'相同。对于存在较多噪声的场景，该参数可
 以设得较大以提高对噪点的鲁棒性。
  *@param numAngles: 在PPF特征检测的'voting scheme' 步骤中，需要对角度进行离散化，从而得
  以使用GHT算法。角度离散化的区间数即为 'numAngles'。参考论文中建议值为'30'，对于存在较多噪
  声的场景，可以将该参数设为 '25' 或 '20' 以提高对噪点的鲁棒性。
 */
cv::ppf_match_3d::PPF3DDetector::PPF3DDetector ( 
    const double 	relativeSamplingStep,
	const double 	relativeDistanceStep = 0.05,
	const double 	numAngles = 30 )

Pyhon: 无 Python 接口

(2) 建立对模型的模型描述

/**
 * @brief: 使用输入的 'Model' 数据，建立一个新的模型
 * @param Model: 模型的3D坐标+法向量点集(Nx6, CV_32F)
 */
void cv::ppf_match_3d::PPF3DDetector::trainModel (const Mat &Model)
    
Python: 无 Python 接口

(3) 匹配

/**
 * @brief: 在提供的场景 'scene' 中，使用以训练的模型进行匹配，并返回匹配得到的所有可能姿态
 * @param scene: 目标场景的3D坐标+法向量点集(Nx6, CV_32F)
 * @param results: 最终求得的姿态列表。
 * @param relativeSceneSampleStep: 相对于场景点集数量的采样步长。如果设为 1.0/5.0，则场
 景点集中的 5-th 的点被用于计算。该参数提供了一种调整算法速度和精度的方法。较大的值可以提高速
 度，但是降低精度。反之，较小的值会提高精度，但降低速度。
 * @param relativeSceneDistance: 相对于模型直径的距离阈值。参数作用类似于训练过程中的 
 'relativeSamplingStep' 参数的作用。
 */
void cv::ppf_match_3d::PPF3DDetector::match	(	
    const Mat & 	scene,
	std::vector< Pose3DPtr > & 	results,
	const double 	relativeSceneSampleStep = 1.0/5.0,
	const double 	relativeSceneDistance = 0.03 )	

Python: 无 Python 接口

3. ICP API

(1) 创建ICP对象

/**
 * @brief: 创建ICP对象
 * @param iterations: 最大迭代次数
 * @param tolerence: 控制ICP算法每次迭代的精度
 * @param rejectionScale: 在ICP算法的 '删除离群点(reject outliers)' 步骤中的scale系数
 * @param numLevels: 金字塔的层数。太深的金字塔层数可以提高计算速度，但最终的精度会降低。过
 于粗略的金字塔，虽然会提高精度，但是在第一次计算时，会带来计算量的问题。一般设在[4, 10]之间内
 较好。
 * @param sampleType: 目前该参数被忽略。
 * @param numMaxCorr: 目前该参数被忽略。
 */
cv::ppf_match_3d::ICP::ICP	(	
    const int 	iterations,
	const float tolerence = 0.05f,
	const float rejectionScale = 2.5f,
	const int 	numLevels = 6,
	const int 	sampleType = ICP::ICP_SAMPLING_TYPE_UNIFORM,
	const int 	numMaxCorr = 1 )		

Python:
<ppf_match_3d_ICP object> =	cv.ppf_match_3d_ICP( iterations[, tolerence[, rejectionScale[, numLevels[, sampleType[, numMaxCorr]]]]]	)

(2) 使用ICP算法对齐场景和模型

/**
 * @brief: 使用 'Picky ICP' 算法对齐场景和模型点，同时返回残差和姿态 
 * @param srcPc/dstPc: 模型/场景3D坐标+法向量集合。大小为(Nx6)，且目前只支持 CV_32F 类型。
 场景和模型点数量不用相同。
 * @param residual: 最终的残差
 * @param pose: 'srcPc' 到 'dstPc' 点集 的变换矩阵
 */
int cv::ppf_match_3d::ICP::registerModelToScene	(	
    const Mat & 	srcPC,
	const Mat & 	dstPC,
	double & 	residual,
	Matx44d & 	pose )

Python:
retval, residual, pose = cv.ppf_match_3d_ICP.registerModelToScene(srcPC, dstPC)

4. ComputeNormals API

(1) 计算3D坐标的法向量

/**
 * @brief: 使用平面拟合的方法，计算一个3D点云中任意点的法向量。
 * @param PC: 输入的3D点云。必须为 (Nx3) 或 (Nx6)
 * @param PCNormals: 输出点云。(Nx6)
 * @param NumNeighbors: 平面拟合时考虑的点的数量
 * @param FlipViewpoint: 如果为 'true'，则计算得到的法向量会被翻转到指向 'viewpoint' 的
 方向。为 'fasle' 则不进行任何操作
 * @param viewpoint: 视点位置
 */
int cv::ppf_match_3d::computeNormalsPC3d ( 
    const Mat & 	PC,
	Mat & 	PCNormals,
	const int 	NumNeighbors,
	const bool 	FlipViewpoint,
	const Vec3f & 	viewpoint )
    
Python:
retval, PCNormals = cv.ppf_match_3d.computeNormalsPC3d(	PC, NumNeighbors, FlipViewpoint, viewpoint[, PCNormals]	)

---