（五）迭代最近点（ICP）
ICP算法
从这里开始，笔者便秉承恩师的教诲，用一些比较规范的写法来介绍算法原理。
已知条件
匹配的一组三维点云:
$$p = \{p_{1}, p_{2}, \dots, p_{n} \}, q = \{q_{1}, q_{2}, \dots, q_{n} \}$$
Tips: （这堆点云一般是根据两帧图像的特征点进行匹配，再利用匹配的像素位置在深度图中查找深度值，利用相机内参以及深度值进行计算相机坐标系下的空间点）。
问题
在上述已知条件下，求解两坨点云的相对位姿（旋转矩阵 $R$ 和位移向量 $t$）。
方法
求解相对位姿的方法，我们可以分解成如下步骤进行：
步骤一：假设两坨匹配点云的相对位姿是 $(R, t)$，则约束关系可以描述成如下形式：
$$p_{i} = Rq_{i} + t, \forall{i} \in [1, n]$$
步骤二：由于实际上我们提供的匹配对数目通常是大于未知数个数的，因此该方程是一个超定方程，这是其一；其二便是我们采集图像深度值的设备存在一定的噪声，无法使得匹配对能完全对准，因此也有一定的误差。因此我们需要用一个误差的形式来表示步骤1中的问题，并将我们的问题转换为最小化误差：
$$E = \sum_{i=1}^{n} || p_{i} - (Rq_{i} + t) ||^{2}$$
步骤三：利用点云的均值可以帮助我们简化问题。（笔者理解，这是为了减少点云偏置的影响，让整个点云不会因为整体偏差太大而和目标点云匹配不上。这有点像NCC基于像素的匹配方法，NCC会要求先减掉整体增加的部分，即取均值，再进行归一化操作，这个目标是为了消除比例光照误差。如果有其他理解的读者，欢迎交流）。假设两坨点云的中心为 $\hat{p}, \hat{q}$。则有：
$$\hat{p} = \frac{1}{n}\sum_{i = 1}^{n}p_{i}, \hat{q} = \frac{1}{n}\sum_{i = 1}^{n}q_{i}$$
我们将步骤2中的问题进行简化：
$$\begin{aligned} E =& \sum_{i=1}^{n} || p_{i} - (Rq_{i} + t) - \hat{p} + \hat{p} - R\hat{q} + R\hat{q}||^{2} \\ =& \sum_{i=1}^{n} || [(p_{i} - \hat{p}) - R(q_{i} - \hat{q})] + (\hat{p} - R\hat{q} - t)||^{2} \\ =& \sum_{i=1}^{n} ||(p_{i} - \hat{p}) - R(q_{i} - \hat{q})||^{2} + \sum_{i=1}^{n}|| \hat{p} - (R\hat{q} + t)||^{2} + 2\sum_{i=1}^{n}[(p_{i} - \hat{p}) - R(q_{i} - \hat{q})][\hat{p} - (R\hat{q} + t)] \end{aligned} $$
步骤四：仔细研究一下步骤3，我们说要把步骤2的公式进行简化，可是看起来结果更加凌乱，并且复杂了？别急，我们来仔细分析一下。比如步骤3公式里的最后一部分可以进行简化：
$$ \begin{aligned} L =& 2\sum_{i=1}^{n}[(p_{i} - \hat{p}) - R(q_{i} - \hat{q})][\hat{p} - (R\hat{q} + t)] \\ =& 2[(p_{1}+p_{2}+ \dots + p_{n} - n\hat{p})-R(q_{1}+q_{2}+\dots+q_{n} - n\hat{q})][\hat{p} - (R\hat{q} + t)] \\ =& 0 \end{aligned} $$
oh man？没了？是的，最后一项相加以后结果为0。可以丢掉了。我们把步骤3的公式简化为如下形式：
$$ \begin{aligned} E =& \sum_{i=1}^{n} ||(p_{i} - \hat{p}) - R(q_{i} - \hat{q})||^{2} + \sum_{i=1}^{n}|| \hat{p} - (R\hat{q} + t)||^{2} \end{aligned} $$
上述公式中第一项只和旋转矩阵 $R$ 相关，而第二项中跟旋转矩阵 $R, t$ 都相关，但是只与点云中心坐标相关。于是，我们可以先根据第一部分计算旋转矩阵，再令第二部分为0,利用旋转矩阵计算位移向量。OK，fine，非常完美的方案。接下来，我们的问题就简化成了两个部分：
$$ E_{1} = \sum_{i=1}^{n} ||(p_{i} - \hat{p}) - R(q_{i} - \hat{q})||^{2} $$
$$ E_{2} = \hat{p} - (R\hat{q} + t) = 0 $$
步骤五：接下来是优化 $E_{1}$，我们令 $p_{i}^{\prime} = p_{i}-\hat{p}$，$q_{i}^{\prime} = q_{i}-\hat{q}$。展开式子：
$$ \begin{aligned} E_{1} =& \sum_{i=1}^{n} ||(p_{i} - \hat{p}) - R(q_{i} - \hat{q})||^{2} \\ =& \sum_{i=1}^{n}(p_{i}^{\prime} - Rq_{i}^{\prime})^{2} \\ =& \sum_{i=1}^{n}((p_{i}^{\prime})^{T}p_{i}^{i} - 2\sum_{i=1}^{n}(p_{i}^{\prime})^{T}Rq_{i}^{\prime} + \sum_{i=1}^{n}(q_{i}^{\prime})^{T}R^{T}Rq_{i}^{\prime} \end{aligned} $$
上述式子第一项与旋转矩阵无关，删掉。第三项中由于 $R^{T}R = I$，因此也与旋转矩阵无关。则优化问题便简化为：
$$ \begin{aligned} E_{1} =& -2\sum_{i=1}^{n}(p_{i}^{\prime})^{T}Rq_{i}^{\prime} \\ =& -2\sum_{i=1}^{n}tr(Rq_{i}^{\prime}(p_{i}^{\prime})^{T}) \\ =& -2tr(R\sum_{i=1}^{n}q_{i}^{\prime}(p_{i}^{\prime})^{T}) \end{aligned} $$
步骤六：优化 $E_{1}$，参考高博提供的参考文献（Arun, K Somani and Huang, Thomas S and Blostein, Steven D, Least-squares fitting of two 3-D point sets, PAMI, 1987.）.令 $W = \sum_{i=1}^{n}q_{i}^{\prime}(p_{i}^{\prime})^{T}$，利用SVD分解可以解得旋转矩阵：
$$R = UV^{T}$$
步骤七：将解得的旋转矩阵代入 $E_{2}$ 中，则可以直接计算出位移向量：
$$t^{\star} = \hat{p} - R\hat{q}$$
步骤八：以SVD求解的结果进一步进行Bundle Adjustment优化，使位姿更精确。
总结
OK，至此，基于SVD分解计算匹配ICP的方法步骤我们已经介绍清楚了，主要是利用点云中心来简化问题，通过先计算旋转矩阵，再利用旋转矩阵计算位移向量的方法完成相对位姿的估计。根据每一个步骤的公式，我们可以很轻松地实现代码，笔者大致实现了一个ICP。笔者博文主要参考高博的知乎，但是在写法上尝试了笔者导师一直引导的描述问题和求解问题的方法。
参考资料
1. 相机位姿求解问题？
2. 点云匹配介绍与ICP算法
3. ICP代码
