一.什么是3D匹配

•形状、大小一致的源点云与目标点云之间的刚体变换。

•源点云通过平移与旋转操作变换到目标点云位置使之重合。

•源点云与目标点云坐标系之间的变换。

典型的应用流程为：

3D相机数据采集----点云生成----点云处理----目标点云提取----3D模板匹配----位姿估计

3D模板匹配的流程为：3D粗匹配----3D精匹配

二.粗匹配的原理（PPF算法）

基于点对特征的匹配过程：

•对源点云所有点对特征进行记录

•遍历目标点云点对，并在记录中查找特征一致的点对。

•通过对应点对求取源点云到目标点云的转换关系。

1. PPF的定义：

(1)两点间的距离：|d|

(2)点1的法线与向量d的夹角

(3)点2的法线与向量d的夹角

(4)点1的法线与点2的法线的夹角

2.off-line准备：

(1)计算模型点云中任意两点的PPF

(2)以其为哈希表的key并在对应的位置存入点对信息

创建的哈希表如下：

PPF	Pairs
F(m1, m2)	(m1, m2)，(m3, m4)，(m7, m9)，(m7, m10)，(m1, m6)......
F(m2, m3)	(m2, m3)，(m3, m5)，(m4, m8)，(m6, m8)，(m7, m8)......
F(m1, m4)	(m1, m4)
......	......
......	......

 3.PPF的步骤

（1）取得点云后，在点云中选择一个参考点

（2）将场景中的点分为参考点（reference point）和其他的点（refered point），在参考点中选择一点，并与其他的点种的每一个点组成的点对且计算PPF

如选择一个参考点Sr1，计算每个点对的PPF，即为计算F(Sr1, Si1)，F(Sr1, Si2)，F(Sr1, Si3)......

（3）用计算的PPF来查哈希表，并将对应的模型点云中的点对抽出

查表的例子：

（4）通过抽出的点对计算位姿（局部坐标系）

将场景的点Sr和模型的点mr转移到世界坐标系原点，将此两点的法线并齐，计算旋转角α

计算位姿的例子：

依次计算每个点对的位姿，得到旋转角α，

 （5）对（位姿，对应的模型点）进行投票

（6）输出最高票的（位姿，对应的模型点）

（7）改变参考点并重复步骤2--6

在完成此步骤后，对于每一个参考点有一个组合F=（参考点，对应的模型的点，位姿，票数）

（8）进行聚类并输出位姿

对F中的位姿进行聚类，计算每个cluster的分数并输出最高票数对应的位姿，分数是每个cluster中所有参考点的票数总和

三.精匹配的原理

精匹配常用的方法有

1.Iterative Closest Point(ICP）

迭代最近点：由于未知点集间对应关系，即认为距离最近的两个点为对应点，所以称为迭代最近点。

求解方法：构建为最小二乘问题，求解使两个点集误差平方和最小的变换（R,T）

方法一：点到点匹配，基于svd分解的解析解 

方法二：点到面匹配，基于非线性优化的迭代解

优点：思路简单，实现容易

缺点：对噪声和异常点比较敏感，鲁棒性较差

2.Coherent Point Drift（CPD）

将点云匹配问题，构建为基于高斯混合模型的极大似然估计，并通过期望最大算法（EM算法）求解。

以每一个模型点的坐标为中心，构建一个高斯核，组成高斯混合模型。每一个场景点，置身于高斯核中，与每一个模型点构成的高斯核有一个对应关系，比如最近点的对应概率可能是0.8，次近点可能是0.19，其它点的对应关系非常小0.01。那对应概率最大的模型点高斯核，即为该场景点的对应点，归一化后的概率值即为点对权重。

通过高斯混合概率分布表达模型点和场景点的对应关系（对应点+权重），而不是ICP硬性假设最近点100%就是对应点。

EM算法建模、求解过程

优化目标：估计一组参数，使场景点云在根据模型云构造的高斯混合模型中后验概率最大，即使模型点和场景点间距均方误差最小。

已知参数：高斯核的位置（均值）

未知参数：高斯核的方差是什么？模型点与场景点之间的对应关系？已知对应关系后，模型点到场景点之间的位姿变换？

EM迭代过程:

第一步，初始化：

根据粗匹配初始位姿，将高斯核初始位置（均值）构建在每个模型点上，根据粗匹配的位姿偏差人工设置一个默认值（比如3毫米）。

第二步，Expection-Step(求解对应关系)：

计算场景点在每个模型高斯核中概率值，取概率最大的高斯核模型点为对应点，归一化后的概率值为点对权重。

第三步，Maximization-Step（求解位姿变换）：

已知模型点与场景点的对应关系(对应点+权重)，构建为最小二乘问题，求解使两个点集误差平方和最小的变换（R,T）

交替进行，获取最佳对应关系和匹配位姿。

优点：鲁棒性优于ICP

缺点：时间复杂度O（mnd），点数稍多，速度极慢，无法在真实应用场景推广应用

3.FilterReg

基本思想：

将点云匹配问题，构建为基于高斯混合模型的极大似然估计，并通过期望最大算法（EM算法）求解。

不同于CPD，为了避免每次迭代后都要重建场景lattice，FilterReg将高斯混合模型构建在场景点上。

CPD取得是概率最大的一个点作为对应点。

FilterReg是周围一定范围内所有点的权重均值。

如何获取模型点对应的场景点（Expectation step）？

通过permutohedral lattice将场景点云构建为高维高斯过滤模型，模型点投影到lattice数据结构中，对模型点3倍标准偏差范围内的场景点做高斯过滤，过滤后的目标点即为模型点的对应点。

如何计算点对权重？

FilterReg中点对权重表达的物理意义：

模型点找到的场景点非outlier point的概率，可以称为点对有效性概率。

计算方式：

weight sum = 模型点在lattice中收集的权重和

点对有效性概率 = weight sum / (weight sum + constant)

从计算公式理解，模型点投影的lattice中，点数越多，距离lattice中心越近，那么权重和越大，点对有效性越高。

4.EM算法收敛性

EM算法为什么能收敛呢？

EM算法收敛性的简易理解角度：坐标上升法（coordinate ascent）。

       图中的直线式迭代优化的路径，可以看到每一步都会向最优值前进一步，而且前进路线是平行于坐标轴的，因为每一步只优化一个变量。

       这犹如在x-y坐标系中找一个曲面的极值，然而曲面函数不能直接求导，因此梯度下降方法就不适用了。但固定一个变量后，另外一个可以通过求导得到，因此可以使用坐标上升法，一次固定一个变量，对另外的求极值，最后逐步逼近极值。

对应到EM上：

E步：固定模型初始位姿，优化点对对应关系；

M步：固定点对对应关系，优化模型位姿； 交替进行将极值推向最大。 

5.点集配准收敛条件

条件一，模型初始位姿临近目标极值，即粗匹配的初始位姿偏差不能过大。

例：曲轴工件，粗匹配给出了反向位姿

条件二，模型点云和场景点云在形状上提供足够的约束

约束刚体位姿需要六个维度，任意一个维度上缺失足够的约束，都无法输出唯一的全局最优解。

例：轴对称物体，下图工件沿X轴对称，所以Y轴和Z轴的方向是在X轴的垂直平面上是随机状态，没有提供足够约束，即在Y轴和Z轴未提供足够的约束。