ICP（Iterative Closest Point迭代最近点）算法学习笔记

背景：博主从百度百科开始学习icp算法，主要是后期加得学习笔记（红色部分）。

ICP算法：以点集对点集（PSTPS）配准方法为基础，他们阐述了一种曲面拟合算法，该算法是基于四元数的点集到点集配准方法。从测量点集中确定其对应的就近点点集后，运用Faugera和Hebert提出的方法计算新的就近点点集。用该方法进行迭代计算，直到残差平方和所构成的目标函数值不变，结束迭代过程。ICP配准法主要用于解决基于自由形态曲面的配准问题。

ICP算法能够通过一定空间变换使不同的空间坐标系坐标下的点云数据统一到到同一个坐标系统中，首先是找到一个可用的变换，配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。

ICP算法本质上：基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对， 计算最优刚体变换，直到满足正确配准的收敛精度要求。

ICP 算法的目的：要找到待配准点云数据与参考云数据之间的旋转参数R和平移参数 T，使得两点数据之间满足某种线性约束下的最优匹配。

ICP算法优点：

• 可以获得非常精确的配准效果
• 不必对处理的点集进行分割和特征提取
• 在较好的初值情况下，可以得到很好的算法收敛性

ICP算法的不足之处： 

• 在搜索对应点的过程中，计算量非常大，这是传统ICP算法的瓶颈
• 标准ICP算法中寻找对应点时，认为欧氏距离最近的点就是对应点。这种假设有不合理之处，会产生一定数量的错误对应点

三维空间R3存在两组含有n个坐标点的点集，分别为： PL和PR。三维空间点集PL中各点经过三维空间变换后与点集PR中点一一对应，其单点变换关系式为：

(0-1)

上式中，R为三维旋转矩阵，t为平移向量。     （R：空间旋转参数）

在ICP配准方法中，空间变换参数向量X可表示为[9] 。参数向量中四元数参数满足约束条件为：

（四元数：一个空间向量，m=t+x*i+y*j+z*k,m是一个超复数，或者说扭量，其中 i^2 = j^2 = k^2 = -1， i^0 = j^0 = k^0 = 1 ，

每个四元数都是 1、i、j 和 k 的线性组合，即是四元数一般可表示为a + bk+ cj + di，其中a、b、c 、d是实数。

对于i、j、k本身的几何意义可以理解为一种旋转，其中i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转，j旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转，k旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转，-i、-j、-k分别代表i、j、k旋转的反向旋转。

）

(0-2)

根据迭代的初值X0，由式(0-1)计算新点集Pi为：

(0-3)

式中，P表示原始未修改过的点集，Pi的下标i表示迭代次数，参数向量X的初始值X0为 。

根据以上数据处理方法，ICP配准算法可以概括为以下七个步骤：

1） 根据点集Plk中的点坐标，在曲面S上搜索相应就近点点集Prk；（前提：点云文件至少3个共同点）

2） 计算两个点集的重心位置坐标，并进行点集中心化生成新的点集；（重心：物体的重力的合力作用点称为物体的重心。(与组成该物体的物质有关)；）

3） 由新的点集计算正定矩阵N，并计算N的最大特征值及其最大特征向量；（线性变换的特征向量（本征向量）是一个非简并的向量，其方向在该变换下不变。该向量在此变换下缩放的比例称为其特征值（本征值）;线性变换的主特征向量是最大特征值对应的特征向量。）

4） 由于最大特征向量等价于残差平方和最小时的旋转四元数，将四元数转换为旋转矩阵R；（扭量就是一个时空点R，需要（t，x，y，z）四个实数来刻画。而这个点的四个实数相对于一个原点,构成了一个四维矢量。使用扭量目的会画画的人多数知道射影几何。当一个画家站在野外写生的时候，画板竖立在面前，画家看到一对平行的铁路线，当在画在纸上的时候，所有跟铁路一起平行的线应该是交于一个点的。这背后的数学就是射影几何。一条光线从远处跑来，它一路经过了很多时空点，但在视网膜上仅仅是同一点。）

5） 在旋转矩阵R被确定后，由平移向量t仅仅是两个点集的重心差异，可以通过两个坐标系中的重心点和旋转矩阵确定；（也就是说通过不同的点云数据，可以确定不同的扭量，即空间中不同的光线，且都是对同一物体的扫描，一定会在空间有重合点）

6） 根据式(0-3)，由点集Plk计算旋转后的点集P’lk。通过Plk与P’lk计算距离平方和值为fk+1。以连续两次距离平方和之差绝对值 作为迭代判断数值；（约束）

7） 当 时，ICP配准算法就停止迭代，否则重复1至6步，直到满足条件 后停止迭代

ICP搜索方法

ICP搜索就近点的主要方法

Point to Point

1. Point to Point就近点搜索法

Point to Point就近点搜索法是ICP算法中最经典的一种方法。如图1a所示, Point to Point法根据源曲面上的一个点p，在目标曲面上找出对应于p点距离最近的q点。在这个方法中通常运用kd-tree的方法实现就近点搜索。如图1b所示，pi是源曲面点云数据中的一个点，Vi是生成目标曲面点云数据中距Pi最近的点。根据Vi点搜索出在曲面上与Vi点相邻的点构成的三角形格网，计算pi点投影到每个三角形平面上的投影点qi的坐标。对于每个三角形来说，当投影点qi位于三角形内部，则距离最近点是搜索的最近点，当投影点qi位于三角形外部，搜索的就近点应位于三角形的两条边界上，Vi是该三角形到pi点的就近距离点。将每个三角形确定的就近距离点进行比较可获得一个最近点。

Point to Plane

2. Point to Plane就近点搜索算法

如图1c所示，Point to Plane法是根据源曲面上的一个点p，在目标曲面上找出对应于p点一个最近的q点。搜索算法是根据源曲面上p点的切平面的法线，确定发现于目标曲面的交点q’。根据目标曲面上q’点求出的过q’点切平面，然后求源曲面上p点到过q’点切平面的垂线的交点q。

Point to Projection

3. Point to Projection就近点搜索算法

Point to Projection就近点搜索法是一种快速的配准方法。如图1-d所示，图中Oq是扫描目标曲面的透视点的位置。Point to Projection法是根据源曲面上的一个点p和透视点Oq，在目标曲面上找出q点作为对应于p点的就近点。通过确定Oq点向p点方向的投影线与目标曲面的交点q，作为搜索的就近点。

参考：

1.百度百科：icp算法

2.https://www.cnblogs.com/21207-iHome/p/6038853.html

三维点云算法

编辑

三维激光扫描技术的快速发展,使其在各个领域得到广泛应用。由于物理上的一些限制,一次三维激光扫描不能获取扫描物体的全部数据,因此要对扫描点云进行拼接。首先,对最常用的ICP算法进行一系列研究,ICP算法的前提条件是具有一个良好的配准初值,文中在配准初值的选取上采用主成分分析法,为后续ICP算法的工作提供一个良好前提条件,增加点集预处理,点对查找上增加各种限制,采用kd-tree加速查找,以此对算法进行改进,并通过实例来验证本算法的有效性及合理性