3D匹配算法简述
一.什么是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轴未提供足够的约束。