ICP算法重读

今天面试的时候被问到了ICP的内容，想想之前自己做了那么多的工作，竟然忘记了，所以决定来重读一下。打算是以Q&A的形式来进行回顾，这样才能清楚的知道到底是哪儿不明白。

Q1:
ICP用的最多的是点云的匹配，一般都是PCL中的ICP算法直接进行实现了，那具体ICP的基本过程到底是怎么样的呢？
A1:
ICP算法包括以下步骤：
1） 根据点集P1中的点坐标，在曲面S上搜索相应最近点点集P2；这个搜索完全就是基于距离的搜索。后期在 此基础上进行kd-tree加速。
2） 计算两个点集的重心位置坐标，并进行点集中心化生成新的两个点集；
3） 由新的两个点集计算矩阵$W=\sum_{i=1}^{n}q_iq_i^{'T}$,并对这个矩阵进行分解为$W=U \sum V^T$,$R=U*V^T$.
4） 得到旋转矩阵和平移矩阵Rt，就可以计算点集P2进行刚体变换之后的新点集P3，由计算P2到P3的距离平方和，以连续两次距离平方和之差绝对值，作为是否收敛的依据。若小于阈值，就收敛，停止迭代。
5）重复a-e，直到收敛或达到既定的迭代次数。

Q2：
基本步骤里面，去中心化的目的是什么呢？
A2：
个人理解，因为在旋转的时候是根据质心旋转的，先去除质心是为了先计算出R，然后才能求出t，两步分开求解。（不知道这个理解对不对）
在一个群里面抛出这个问题之后，引起了一点争论：
观点一：去中心让函数更凸，更容易收敛。（我个人不是很明白这个意思）
因为：一个去中心化，不过是做了一个平移。何以做到让原本“绝大多数”的“凹的”东西更凸？
观点二：好像就是认同我的这个观点了。

Q3:
那么找到匹配点，去中心化之后，怎么计算两个点之间的旋转R和平移t呢？
A3：
假设找到
$P= \{ p_1,p_2,...,p_n \}$的匹配点$P^{'}= \{ p^{'}_1,p^{'}_2,...,p^{'}_n \}$那么两者之间的关系就会用如下方程式表示：

$$e_i = p_i-(Rp^{'}_i+t)$$
然后根据所有的匹配点构造最小二乘问题，求使得误差平方和达到极小的$R,t$.
$$\min_{R,t} J = \frac{1}{2} \sum_{i=1}^n ||(p_i - (Rp^{'}_i+t))||{^2_2}$$
求解这个问题，分为三步：

1. 计算两组点的质心位置$p,p^{'}$,公式如下：
   $$p = \frac{1}{n} \sum_{i=1}^n(p_i)$$
   $$p^{'} = \frac{1}{n} \sum_{i=1}^n(p^{'}_i)$$
   那么误差函数可以写成：
   $$\frac{1}{2} \sum_{i=1}^n ||(p_i - (Rp^{'}_i+t))||{^2} = \frac{1}{2} \sum_{i=1}^n ||(p_i - Rp^{'}_i-t-p+Rp^{'}+p-Rp^{'}))||{^2}$$
   最终化解之后得到如下目标函数：
   $$\min_{R,t} J = \frac{1}{2} \sum_{i=1}^n ||p_i-p - R(p^{'}_i-p^{'}))||{^2}+||p-Rp^{'}-t||$$
   上诉公式中的两部分，前面部分只与$R$有关，后面部分和两个都相关，那么整体优化的话是不是可以转为先优化前面部分，然后再优化后面部分。
2. 根据第一步求出的改为先优化前面部分，即如下：
   $$R^* = arg \min_{R} \frac{1}{2} \sum_{i=1}^n||q_i -Rq^{'}_i||^2$$
3. 再根据第二步求出的$R$,来算出$t$.
   $$t^* = p-Rp^{'}$$

Q4:
那么在Q3中的第二步，如何求解R呢？
A4：
解法一：
化解Q3中的第二步，得到如下公式：

$$\frac{1}{2} \sum_{i=1}^n||q_i -Rq^{'}_i||^2 = \frac{1}{2} \sum_{i=1}^n q_i^Tq_i+q_i^{'T}R^TRq_i^{'}-2q_i^TRq_i^{'}$$
其中的第一项和R无关，第二项由于$RR^T=I$，所以最后只剩下最后一项$\sum_{i=1}^n -q_i^TRq_i^{'}$,根据矩阵运算的关系可知：
$$\sum_{i=1}^n -q_i^TRq_i^{'}=\sum_{i=1}^n -tr(Rq_i^{'}q_i^T) = -tr(R\sum_{i=1}^n q_i^{'}q_i^T)$$
接下来就是SVD求解R的过程了，先令上式中除R的部分未$W=\sum_{i=1}^n q_i^{'}q_i^T$,$W$是一个3*3的矩阵,对$W$进行SVD分解，得到如下公式 $$W=U \sum V^T$$ ,其中的$\sum$是奇异值组成的对角矩阵，对角线元素从大到小排列，而$U$ 和$V$为对角矩阵，当$W$满秩的时候， $$R=UV^T$$ ,于是便可以解出R了，然后再根据R求解t.

解法二：
使用非线性优化的方法。以迭代的方式去找最优解。
目标函数写成：

$$\min_\varepsilon = \frac{1}{2}\sum_{i=1}^n || (p^i-exp(\varepsilon)p_i^{'})$$
这就转为了四元数表示的旋转和平移了。求解四元素的旋转与平移就可以使用求导操作，然后转为李代数的扰动模型来求，这样会更简化一些。

Q5:
ICP的优缺点是什么呢？
A5：

• 在较好的初值情况下，可以得到很好的算法收敛性。但是这也是一个缺点，就是初始值不好的话，结果也会受影响。
• 在找匹配点的时候，认为距离最近的点就是对应点，这点比较果断。CPD会在每个距离之前加上权值，所以相比ICP有一定的优化效果。
• ICP对于形状相似，但是角度偏差比较大的情况，匹配效果比较差。因为是基于最近点找的。CPD的话是全局的考虑，如果是局部和全局进行匹配的话，CPD不一定能取得比ICP更好的结果。