ICP（Iterative Closest Point）点云配准

通常，点云配准分为两步，先做粗配准，再做精配准：

• 粗配准（Coarse Global Registeration）：基于局部几何特征
• 精配准（Fine Local Registeration）：需要初始位姿（initial alignment）

ICP 的经典论文：P.J. Besl, A method for registration of 3-D shapes, 1992.

迭代最近点（ICP，Iterative Closest Point）算法是一种点云匹配算法。也就是想要做到一件事情：通过平移和旋转使得两个点云三维模型重合。

1、问题构建

假设我们通过某种方法获得第一组点云p = {p1, p2, p3, ..., pn}, 然后经过相机变换之后获得了另一组点云集合Q = {q1, q2, q3, ..., qn},

那么就存在P集合中的任意一点i，经过旋转和平移能够变换到Q中去：

于是就引出了ICP优化问题，该优化问题是基于最小二乘法，使得点云的误差平方和达到最小值。公式如下图所示：

我们观察这个公式，这个公式其实就是神经网络中MSE损失函数，我们希望能够通过优化求解一个R和t，使得这个平方和最小。可以通过一步一步迭代的方法求解，那么这就是ICP算法。

2、优化问题的求解

上面，我们定义了一个优化方程：

求解极大值极小值的方法就是求导，在等于另的时候，就是解了。那么这个方程里面有两个需要求解的未知数，我们首先求解平移距离t（translation）

（1）translation平移

通过对t求偏导，解方程，最终我们得到了上面的的解。观察上面的式子，我们可以看到其实是对P和Q都求了一个平均，于是我们可以定义一个质心，来简化这个公式：

我们得到了最终解，我们将解带入到原式中进行求解，可得：

同样的，为了简化公式，我们再定义了一个去质心坐标（能够让数据值域更好）：

此时，我们的目标函数变成了这个样子，也没有个t这个变量，此时目标函数中只有R是未知数，那么我们需要求解R。

（2）rotation旋转

上述的目标函数均为矩阵运算，根据矩阵的运算法则，可以进行如下推导：（个人感觉像是矩阵计算的完全平方公式？）

带入原式中，把常数项去掉：

这里将求最小值问题转换成了求最大值问题

这里补充几个小知识点：方阵A的迹tr(A)=a11+a22+...+ann，即等于对角线元素和。设有N阶矩阵A，那么矩阵A的迹（用 tr(A) 表示）就等于A的特征值的总和，也即矩阵A的主对角线元素的总和。

协方差矩阵的复习：https://blog.csdn.net/qq_41076797/article/details/112640676

SVD的原理和意义可以看这个进行复习：https://zhuanlan.zhihu.com/p/36546367

补充完小知识点，可以进行接下来的推导：

由于矩阵的迹就是对角线之和，就变成了上述公式的样子，然后由于m都是小于等于1的值，所以当m取1的时候tr(A)得到最大值。又因为M = V^TRU所以最后推出,R = VU^T。

得出R之后t也就能够求出了。