前言

通常，使用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、四元数

我们可以使用四元数表示旋转关系： qR→=[q0q1q2q3]T q R → = [ q 0 q 1 q 2 q 3 ] T <script type="math/tex" id="MathJax-Element-55">\vec{q_R} = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T</script>，满足条件： q0≥0 q 0 ≥ 0 <script type="math/tex" id="MathJax-Element-56">q_0 \geq 0</script>、 q20+q21+q22+q23=1 q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 <script type="math/tex" id="MathJax-Element-57">q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1</script>。
由于点的坐标是用 [xyz]T [ x y z ] T <script type="math/tex" id="MathJax-Element-58">\begin{bmatrix} x & y & z \end{bmatrix}^T</script>表示，如果要对其做某个旋转或者平移变换，无法也用四元数来表示，所以要通过下面的公式得到四元数所表示的旋转矩阵：

<script type="math/tex; mode=display" id="MathJax-Element-59">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}</script>
注：虽然四元数相对难理解一些，但是稍加对比就能发现，使用四元数来表示旋转关系能减少很多参数。迭代时可是要多次用到这个旋转关系的，可以节约不少计算量。这样，我们只需要在最后通过上式将四元数表示的旋转关系转换回旋转矩阵即可。
平移向量可以表示为：
<script type="math/tex; mode=display" id="MathJax-Element-60">T = \begin{bmatrix} q_4 & q_5 & q_6 \end{bmatrix}^T \tag{2}</script>

2、算法流程

1、假设待配准的点集为： P={pi→} P = { p i → } <script type="math/tex" id="MathJax-Element-322">P = \{ \vec{p_i} \}</script>；模板点集为： X={xi→} X = { x i → } <script type="math/tex" id="MathJax-Element-323">X = \{ \vec{x_i} \}</script>。
接下来要从带配准的点集 P P <script type="math/tex" id="MathJax-Element-324">P</script>中提取出与模板点集X $X$<script type="math/tex" id="MathJax-Element-325">X</script>中的每个点最近的匹配点，度量方式采用欧式距离。对于模板点集 X X <script type="math/tex" id="MathJax-Element-326">X</script>中的每个点，求其到到点集P $P$<script type="math/tex" id="MathJax-Element-327">P</script>中每个点的欧氏距离，取出欧氏距离最小的那个点，作为对应点放入新的对应点集 P′ P ′ <script type="math/tex" id="MathJax-Element-328">P^{'}</script>。为表述简洁，第2节开始后面提到的点集 P P <script type="math/tex" id="MathJax-Element-329">P</script>默认都是指对应点集P′ $P^{{}^{\prime }}$<script type="math/tex" id="MathJax-Element-330">P^{'}</script>。
注：原论文中说这一步要保证两个点集 P P <script type="math/tex" id="MathJax-Element-331">P</script>和X $X$<script type="math/tex" id="MathJax-Element-332">X</script>的点的个数相同： Nx=Np N x = N p <script type="math/tex" id="MathJax-Element-333">N_x=N_p</script>，实际使用中可以不必保证这一点。因为模板点集是固定的，我们所要做的只是计算模板点集中的 Np N p <script type="math/tex" id="MathJax-Element-334">N_p</script>个点对应点集 P P <script type="math/tex" id="MathJax-Element-335">P</script>中的点，即欧式距离最小的点，点集P $P$<script type="math/tex" id="MathJax-Element-336">P</script>中的点的个数可以不等于 Np N p <script type="math/tex" id="MathJax-Element-337">N_p</script>。但是对应点集 P′ P ′ <script type="math/tex" id="MathJax-Element-338">P^{'}</script>的点与点集 X X <script type="math/tex" id="MathJax-Element-339">X</script>中的点一一对应，其点的个数肯定等于Np $N_p$<script type="math/tex" id="MathJax-Element-340">N_p</script>。而实际使用中，两个点集往往也是不相等的。

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

<script type="math/tex; mode=display" id="MathJax-Element-341">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}</script>
其中， q⃗  q → <script type="math/tex" id="MathJax-Element-342">\vec{q}</script>表示四元数变量， Rq⃗  R q → <script type="math/tex" id="MathJax-Element-343"> R_{\vec{q}}</script>为将四元数转换为旋转矩阵的形式， T T <script type="math/tex" id="MathJax-Element-344">T</script>是前面定义过的平移矩阵；xi→ $\stackrel{\to }{x_i}$<script type="math/tex" id="MathJax-Element-345">\vec{x_i}</script>表示模板点集 X X <script type="math/tex" id="MathJax-Element-346">X</script>中的某个点的坐标，pi→ $\stackrel{\to }{p_i}$<script type="math/tex" id="MathJax-Element-347">\vec{p_i}</script>表示对应点集 P′ P ′ <script type="math/tex" id="MathJax-Element-348">P^{'}</script>中的对应点的坐标。
求解ICP的过程，就是求解上面这个目标函数最小值的过程： minf(q⃗ ) min f ( q → ) <script type="math/tex" id="MathJax-Element-349">\min{f(\vec{q})}</script>。每次迭代过程都求解能使得 f(q⃗ ) f ( q → ) <script type="math/tex" id="MathJax-Element-350">f(\vec{q})</script>更小的 q⃗  q → <script type="math/tex" id="MathJax-Element-351">\vec{q}</script>，迭代收敛后返回的 q⃗  q → <script type="math/tex" id="MathJax-Element-352">\vec{q}</script>即为最终配准后的结果。

3、设点集 P P <script type="math/tex" id="MathJax-Element-353">P</script>和点集X $X$<script type="math/tex" id="MathJax-Element-354">X</script>的中心点分别为 μp→ μ p → <script type="math/tex" id="MathJax-Element-355">\vec{\mu_p}</script>和 μx→ μ x → <script type="math/tex" id="MathJax-Element-356">\vec{\mu_x}</script>：

<script type="math/tex; mode=display" id="MathJax-Element-357">\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}</script>
对点集 P P <script type="math/tex" id="MathJax-Element-358">P</script>和点集X $X$<script type="math/tex" id="MathJax-Element-359">X</script>做去中心化处理后，求出其协方差矩阵：
<script type="math/tex; mode=display" id="MathJax-Element-360">\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}</script>
令对称矩阵：
<script type="math/tex; mode=display" id="MathJax-Element-361">A_{ij} = (\Sigma_{ij} - \Sigma_{ij}^T)_{ij} \tag{6}</script>
由此得到列向量：
<script type="math/tex; mode=display" id="MathJax-Element-362">\Delta = \begin{bmatrix} A_{23} & A_{31} & A_{12} \end{bmatrix}^T \tag{7}</script>
利用该列向量构建 4×4 4 × 4 <script type="math/tex" id="MathJax-Element-363">4 \times 4</script>对称矩阵：
<script type="math/tex; mode=display" id="MathJax-Element-364">Q = \begin{bmatrix} tr(\Sigma_{px}) & \Delta^T \\ \Delta & \Sigma_{px}+\Sigma_{px}^T-tr(\Sigma_{px}) I_3 \end{bmatrix} \tag{8}</script>
其中， tr(Σpx) t r ( Σ p x ) <script type="math/tex" id="MathJax-Element-365">tr(\Sigma_{px})</script>表示矩阵 Σpx Σ p x <script type="math/tex" id="MathJax-Element-366">\Sigma_{px}</script>的迹，即主对角线元素的总和，也即特征值之和。 I3 I 3 <script type="math/tex" id="MathJax-Element-367">I_3</script>表示 3×3 3 × 3 <script type="math/tex" id="MathJax-Element-368">3 \times 3</script>的单位矩阵。
将式子7代入式子8中：

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

4、对式子9中的矩阵 Q Q <script type="math/tex" id="MathJax-Element-369">Q</script>作特征值分解，求得最大的特征值以及其对应的特征向量，那个特征向量就对应误差的平方和最小时的四元数。（这个我也不是很清楚，为什么这个特征向量就对应误差平方和最小时的四元数？？？）

<script type="math/tex; mode=display" id="MathJax-Element-370">\vec{q} = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T \tag{9}</script>
使用上式的四元数套入公式1：

<script type="math/tex; mode=display" id="MathJax-Element-371">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}</script>
可以求得这次迭代的旋转矩阵。
旋转矩阵确定后，平移矩阵很容易确定了，就是两个点集中心做平移。
<script type="math/tex; mode=display" id="MathJax-Element-372">T = \vec{\mu_x} - R_{\vec{q_R}} \vec{\mu_p}</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-373">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}</script>

实验

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

使用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