迭代最近点（Iterative Closest Point, ICP）算法及matlab实现

前言

通常，使用RGB-D相机或是其他方法获取到物体的三维点云后，由于采集设备不同、拍摄视角不同等等因素的影响，即使是同一个物体所得到的点云也会有较大的差异，主要是旋转或者平移的变化。对于一组图像数据集中的两幅图像，需要通过寻找一种空间变换把一幅图像映射到另一幅图像，使得两图中对应于空间同一位置的点一一对应起来，从而达到信息融合的目的。所以，就需要对点云进行配准。
迭代最近点算法（ICP）是一种点云匹配算法。其思想是：通过旋转、平移使得两个点集之间的距离最小。ICP算法由Besl等人于1992年提出，文献可以参考：A Method for Registration of 3D Shapes，另外还可以参考：Least-Squares Fitting of Two 3-D Point Sets。前者使用的是四元数方法来求解旋转矩阵，而后者则是通过对协方差矩阵求解SVD来得到最终的旋转矩阵。流程大体上一致，后面就主要列出前者的流程。

ICP

1、四元数

我们可以使用四元数表示旋转关系：$\vec{q_R} = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T$，满足条件：$q_0 \geq 0$、$q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1$。
由于点的坐标是用$\begin{bmatrix} x & y & z \end{bmatrix}^T$表示，如果要对其做某个旋转或者平移变换，无法也用四元数来表示，所以要通过下面的公式得到四元数所表示的旋转矩阵：

$$R_{\vec{q_R}} = \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_2q_3) & 2(q_1q_3+q_0q_2) \\ 2(q_1q_2 + q_2q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2q_3 - q_0q_1) \\ 2(q_1q_3-q_0q_2 & 2(q_2q_3+q_0q_1) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix} \tag{1}$$
注：虽然四元数相对难理解一些，但是稍加对比就能发现，使用四元数来表示旋转关系能减少很多参数。迭代时可是要多次用到这个旋转关系的，可以节约不少计算量。这样，我们只需要在最后通过上式将四元数表示的旋转关系转换回旋转矩阵即可。
平移向量可以表示为：
$$T = \begin{bmatrix} q_4 & q_5 & q_6 \end{bmatrix}^T \tag{2}$$

2、算法流程

1、假设待配准的点集为：$P = \{ \vec{p_i} \}$；模板点集为：$X = \{ \vec{x_i} \}$。
接下来要从带配准的点集$P$中提取出与模板点集$X$中的每个点最近的匹配点，度量方式采用欧式距离。对于模板点集$X$中的每个点，求其到到点集$P$中每个点的欧氏距离，取出欧氏距离最小的那个点，作为对应点放入新的对应点集$P^{'}$。为表述简洁，第2节开始后面提到的点集$P$默认都是指对应点集$P^{'}$。
注：原论文中说这一步要保证两个点集$P$和$X$的点的个数相同：$N_x=N_p$，实际使用中可以不必保证这一点。因为模板点集是固定的，我们所要做的只是计算模板点集中的$N_p$个点对应点集$P$中的点，即欧式距离最小的点，点集$P$中的点的个数可以不等于$N_p$。但是对应点集$P^{'}$的点与点集$X$中的点一一对应，其点的个数肯定等于$N_p$。而实际使用中，两个点集往往也是不相等的。

2、对于两个点集之间的度量，使用如下目标函数表示：

$$f(\vec{q}) = \frac{1}{N_p} \sum_{i=1}^{N_p} || \vec{x_i} - R_{\vec{q}} \vec{p_i} - T ||^2 \tag{3}$$
其中， $\vec{q}$表示四元数变量， $R_{\vec{q}}$为将四元数转换为旋转矩阵的形式， $T$是前面定义过的平移矩阵；$\vec{x_i}$表示模板点集 $X$中的某个点的坐标，$\vec{p_i}$表示对应点集 $P^{'}$中的对应点的坐标。
求解ICP的过程，就是求解上面这个目标函数最小值的过程： $\min{f(\vec{q})}$。每次迭代过程都求解能使得 $f(\vec{q})$更小的 $\vec{q}$，迭代收敛后返回的 $\vec{q}$即为最终配准后的结果。

3、设点集$P$和点集$X$的中心点分别为$\vec{\mu_p}$和$\vec{\mu_x}$：

$$\begin{cases} \vec{\mu_p} = \frac{1}{N_p} \sum_{i=1}^{N_p} \vec{p_i} \\ \vec{\mu_x} = \frac{1}{N_p} \sum_{i=1}^{N_p} \vec{x_i}\end{cases} \tag{4}$$
对点集 $P$和点集$X$做去中心化处理后，求出其协方差矩阵：
$$\Sigma_{px} = \frac{1}{N_p} \sum_{i=1}^{N_p} [(\vec{p_i} - \vec{u_p}) (\vec{x_i} - \vec{u_x})^T] = \frac{1}{N_p} \sum_{i=1}^{N_p} (\vec{p_i} \vec{x_i}^T) - \vec{\mu_p} \vec{\mu_x}^T \tag{5}$$
令对称矩阵：
$$A_{ij} = (\Sigma_{ij} - \Sigma_{ij}^T)_{ij} \tag{6}$$
由此得到列向量：
$$\Delta = \begin{bmatrix} A_{23} & A_{31} & A_{12} \end{bmatrix}^T \tag{7}$$
利用该列向量构建 $4 \times 4$对称矩阵：
$$Q = \begin{bmatrix} tr(\Sigma_{px}) & \Delta^T \\ \Delta & \Sigma_{px}+\Sigma_{px}^T-tr(\Sigma_{px}) I_3 \end{bmatrix} \tag{8}$$
其中， $tr(\Sigma_{px})$表示矩阵 $\Sigma_{px}$的迹，即主对角线元素的总和，也即特征值之和。 $I_3$表示 $3 \times 3$的单位矩阵。
将式子7代入式子8中：

程序中直接套用上面这个公式即可。

4、对式子9中的矩阵$Q$作特征值分解，求得最大的特征值以及其对应的特征向量，那个特征向量就对应误差的平方和最小时的四元数。（这个我也不是很清楚，为什么这个特征向量就对应误差平方和最小时的四元数？？？）

$$\vec{q} = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T \tag{9}$$
使用上式的四元数套入公式1：

$$R_{\vec{q_R}} = \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_2q_3) & 2(q_1q_3+q_0q_2) \\ 2(q_1q_2 + q_2q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2q_3 - q_0q_1) \\ 2(q_1q_3-q_0q_2 & 2(q_2q_3+q_0q_1) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix} \tag{1}$$
可以求得这次迭代的旋转矩阵。
旋转矩阵确定后，平移矩阵很容易确定了，就是两个点集中心做平移。
$$T = \vec{\mu_x} - R_{\vec{q_R}} \vec{\mu_p}$$

5、计算目标函数如下式，如果结果小于阈值则停止迭代，否则继续重复前面的步骤1-4；

$$f(\vec{q}) = \frac{1}{N_p} \sum_{i=1}^{N_p} || \vec{x_i} - R_{\vec{q}} \vec{p_i} - T ||^2 \tag{3}$$

实验

实验的代码我会贴在文末，下面是实验结果。
点云配准前：

使用ICP算法配准后：

ICP最终得到旋转矩阵和误差：

代码

我已将代码上传到了github上，请自行下载。
https://github.com/ToughStoneX/3D_ICP

参考资料

1、https://blog.csdn.net/u013832676/article/details/44316107
2、https://blog.csdn.net/u011772859/article/details/44885675
3、https://wenku.baidu.com/view/08833045d15abe23482f4da4.html