ICP算法学习笔记（原理加公式理解）

近期阅读NICP. Dense Normal Based Point Cloud Registration论文之ICP算法理解

ICP算法是点集对点集的配准方法。此论文中有比较两三种配准算法（逐步阅读论文会加入对于GICP、NICP等的学习），这些算法的误差度量和数据关联方式是不同的，通过不同的误差度量方法和数据关联方法可以拥有不同的ICP算法，这也可以给我提供一种思路，可以通过改进误差度量和数据关联来改进优化算法。

ICP算法的误差度量是三维点到点之间的欧式距离（在欧几里得空间，是两点之间或多点之间距离的表示。对比马氏距离在文章结尾）。数据对应关系方法很多，比如可以通过相机跟踪（点对点利用图像来确定对应关系）。

注册两个点云的问题在于找到旋转矩阵和平移矩阵，使得目标函数（核心最小二乘法）最小化，使得两个点云之间的重叠最大化。

公式来自于NICP. Dense Normal Based Point Cloud Registration论文

T∗为两个场景中对应点之间距离最小的变换矩阵；

T是Pr映射到Pc坐标系中转换矩阵的估计，是非常重要的，将一个点云中的点转换到另一个点云中；

Ω是传感器或者表面噪声的信息矩阵（我理解的是外部干扰）；

C={<i，j>1:m}是两个点云之间的一组对应点；

⊕是一个将变换矩阵T应用到点集上的算子，T⊕Pi（某个点）可以化为转换矩阵与向量的乘积；

argminΣ：求和最小值；

本式子中我有一个疑惑：式子中矩阵的转置乘以矩阵本身的意义是什么？看了知乎还不是很理解，之后会加入。可以参考此文章矩阵的转置乘以矩阵本身的意义是什么？ - 知乎

ICP算法中有对目标函数最小化的公式求解旋转矩阵和平移矩阵的推导，推导还是比较好理解的，可以跟着此视频一步一步推导，其中有关于去质心的问题，我认为是借鉴了法向量估计（最小二乘拟合平面），平面过重心处理式子。ICP算法粗陋介绍_哔哩哔哩_bilibili

ICP数据关联对应关系可以用最近邻点来找（之前有接触过KD树），离一个点最近的点就是关联点。ICP算法通过搜索对应关系和求解上述（1）的式子来迭代地改进变换矩阵T，每一新步骤中通过考虑最近点地计算，重新计算对应关系。

迭代的官方解释：迭代是重复反馈过程的活动，其目的通常是为了逼近所需目标或结果。每一次对过程的重复称为一次“迭代”，而每一次迭代得到的结果会作为下一次迭代的初始值。特别是这种初始变换值是对ICP算法的鲁棒性有很大的影响，记得之前粗略看过一篇论文，介绍的是与ICP算法对应的一种算法，关于初始值的改进。

ICP算法的鲁棒性和性能还与信息矩阵Ω或是寻找对应关系的方法的选择而受到影响。1、若Ω为恒等式，沿着法线方向选择一个具有零特征值的Ω，使点到平面的度量最小化。（对于零特征值，我搜索知乎得到我能够理解的解释：对于三维空间来说，变换使得空间压缩到一条线上，会存在一个平面，使得上面所有的向量收缩到0向量，这个平面向量的集合叫做0空间），2、寻找对应关系可以通过考虑点的邻域来考虑。

名词解释：

马氏距离：表示数据的协方差距离，有效的计算两个未知样本集相似度的方法，与欧式距离不同的是它考虑到各种特性之间的联系。比如一个人的身高和体重，一条身高信息会带来一条关于体重信息，因为这两者是有关系的并且尺度无关。

具体可参考马氏距离和欧式距离详解_From Zero to Hero-CSDN博客_欧式距离