PCL 点云配准数学原理

学习目的：

能够大致看懂技术算法的论文（理论理解能力）
能够将论文和代码对应起来（代码追踪能力）
知道常见算法函数的代码实现方式（算法实现能力）
注意！不要求能够推导、证明算法论文，因为这是理论研究者的重心。这不是我们应用型人才的重心，我们的重心是能够将理论更好的应用于实践。一定要有的放矢，提升自己的核心价值。

概念基础
三维点云的配准即是通过对点云的旋转、平移进行对齐，进而得到一个4x4的变换矩阵，包含3x3的旋转矩阵和3x1的平移向量。旋转矩阵可以通过罗格里格斯变换成一个1x3的旋转向量，故而最终是6个变量。

常用的点云配准算法是：迭代最近点算法(ICP)，正态分布变换配准(NDT)，基于特征不变性的配准 (ORB/FPFH/NARF)

其他演变算法及不同思路算法：

ICP拓展：稳健ICP、point to plane ICP、point to line ICP、MBICP、GICP

NDT拓展：NDT 3D、Multil-Layer NDT

其他：4PCS、Super-4PCS、FPCS、KFPSC、SAC-IA、Line Segment Matching、ICL

ICP求解：四元数求解获得旋转矩阵、协方差矩阵求解SVD获得旋转矩阵
KD tree优化
NDT：probability density functions：PDF概率密度函数
矩阵：正交矩阵，协方差矩阵、雅克比矩阵、黑塞矩阵
概率统计：方差，标准差，正态分布，均匀分布
迭代最近点算法(ICP)
本例我们讲解基于奇异值分解（SVD）的方法求解迭代最近点算法（ICP）。目的是匹配两个点云集合。算出他们之间的变换关系。

除此之外还有基于四元数求解ICP。

问题描述
假如我们有两个3D点云集合作为输入：

p={p1,p2,…,pn} 和 q={q1,q2,…,qn}

要求得一个旋转矩阵 R(3×3) 和一个平移向量 t(3×1) 使得：

pi=Rqi+t

即，将点云 q 通过旋转、平移到目标 p，由于两朵点云可能不是完全相同的，也可能是从不同角度获取的相同目标的点云，所以往往我们不能得到一个准确的R和t使以上等式成立。但是我们可以让经过变换后的点云 q 尽可能的靠近 p ，即我们可以定义每一项的点对误差项：

ei=pi−(Rqi+t)

进而构建最小二乘问题，将之转化为目标函数的优化问题，求以使以下目标函数误差的平方和达到极小时的R和t：

E(R,t)=arg minR,t12 n∑i=1 ‖
确定目标函数
求旋转R和平移t的关系

求解步骤
1、求最大化目标函数

2、矩阵进行SVD奇异值分解

3、正定矩阵迹的性质

4、根据迹性质得出目标函数最大化时R的解

5、ICP结果注意

正态分布变换配准(NDT)
NDT表示目标表面
直接使用点云来表示目标的表面是不够的，比如，点云不能显式的表示关于表面的信息（方向、平滑度、孔洞等）。根据数据采集器的不同，点云的内容也可能是低效的，需要大量不必要的存储信息。为了在远离数据采集器的地方能够得到足够的样本分辨率，通常需要采集大量的冗余数据。

正态分布变换（ Normal Distributions Transform ， NDT）是一种描述曲面的较好方式。该变换将点云映射到平滑的表面，通过一组局部概率密度函数 local probability density functions(PDFs)来描述，每个PDF描述了表面的一个小局域的形状。

该算法的第一部是将扫描到的内容所占据的空间分割成网格单元cells（2D下为正方形，3D下为正方体），然后基于每个cell内部点的分布，计算其PDF。每个单元cell中的PDF可以理解为内部点\vec{x}的生成器。换句话说，我们可以认为\vec{x}的位置是由PDF生成的。

图6.1：一个2D的激光扫描仪在矿井坑道中扫描到的点，以及对应的PDFs描述的表面形状。本例中，每个小格子边长是2m，发光区域表示了高概率区域。PDFs只计算那些点数量大于5个的单元cell。

图6.2：3D-NDT表面表示的隧道截面，从上面看。更亮、更密集的部分表示更高的概率。cell的边长为1米

正态分布给出了具有连续导数的点云的分段光滑表示。每个PDF都可以看作是局部表面的近似，描述了表面的位置以及它的方向和平滑度。二维激光扫描及其对应的正态分布如图6.1所示。图6.2描述了矿井巷道扫描的三维常规分布。

正态分布

由于目前的工作主要集中在正态分布上，让我们更仔细地研究一下单变量和多变量正态分布的特征。在一维情况下，一个标准随机正态分布的变量x具有一个确定的预期变量\mu（均值）和一个用来表示不确定性的变量\sigma（方差）。

如方程6.1所描述的多元概率函数p(\vec{x})在一维的情况下即可以退化到以上6.4的函数。在多维情况下，均值和方差将以均值向量 \vec{\mu} 和协方差矩阵 \Sigma 的形式描述。协方差矩阵的对角元素表示每个变量的方差，非对角元素表示变量的协方差。图6.3说明了在一维、二维、三维情况下的正态分布。

在2D和3D情况下，表面的方向信息和平滑度信息可以从协方差矩阵的特征向量和特征值中估算出来，特征向量描述了分布的主要部分：即一个正交向量集合，对应于各变量协方差的主方向。根据方差的比例，二维正态分布可以是点形(如果方差相似)或线形(如果一个比另一个大得多)，或者介于两者之间。在三维情况下，参见图6.4的描述，一个正态分布可以表现为一个点或球体（如果方差的大小在各个方向上差不多），一条线（如果一个方向的方差远远大于另两个方向）或一个平面（如果一个方向的方差远小于另两个方向）。

NDT扫描配准
scan registration

当我们使用NDT技术对扫描数据进行配准时，我们的目标是找到一个当前扫描数据Source的位姿变换，这个变换可以使当前扫描点云表面与参考点云表面Target尽可能的重叠（使似然函数最大化）。

图 6.5：对比一个正态分布p(x)和混合模型\bar{p}(x)。负对数似然函数-log(p(x))也可以是进行NDT扫描配准时的目标函数，它的导数表征了特定测量值对结果的偏置。

对于正态分布p(x)，较大的x值对结果的影响过大（没有边界），但是对于混合模型\bar{p}(x)就没那么大（有边界）。
牛顿法就是用一个二次曲面去拟合你当前所处位置的局部曲面，而梯度下降法是用一个平面去拟合当前的局部曲面，通常情况下，二次曲面的拟合会比平面更好，所以牛顿法选择的下降路径会更符合真实的最优下降路径。下图中红线表示牛顿法，绿线表示梯度下降法。

黑塞矩阵\mathrm{H}的中心函数H_{ij} 为：

匹配准流程