ICP（迭代最近点）算法（转载并加入个人理解；如有不对，请指正）

参考:https://blog.csdn.net/zhouyelihua/article/details/77807541（推导的很详细）

       https://blog.csdn.net/u010312937/article/details/81367321

      https://blog.csdn.net/jsgaobiao/article/details/78873718(这个写的不错)

ICP（迭代最近点）算法介绍和基本原理

图像配准是图像处理研究领域中的一个典型问题和技术难点，其目的在于比较或融合针对同一对象在不同条件下获取的图像，例如图像会来自不同的采集设备，取自不同的时间，不同的拍摄视角等等，有时也需要用到针对不同对象的图像配准问题。具体地说，对于一组图像数据集中的两幅图像，通过寻找一种空间变换把一幅图像映射到另一幅图像，使得两图中对应于空间同一位置的点一一对应起来，从而达到信息融合的目的。 一个经典的应用是场景的重建，比如说一张茶几上摆了很多杯具，用深度摄像机进行场景的扫描，通常不可能通过一次采集就将场景中的物体全部扫描完成，只能是获取场景不同角度的点云，然后将这些点云融合在一起，获得一个完整的场景。

　　ICP（Iterative Closest Point迭代最近点）算法是一种点集对点集配准方法。如下图所示，PR（红色点云）和RB（蓝色点云）是两个点集，该算法就是计算怎么把PB平移旋转，使PB和PR尽量重叠。

用数学语言描述如下，即ICP算法的实质是基于最小二乘法的最优匹配，它重复进行“确定对应关系的点集→计算最优刚体变换”的过程，直到某个表示正确匹配的收敛准则得到满足。思想如下:

1、给定两组待匹配的点集(云) 

2、根据一定的规则找到目标点云和源点云中的最近临近点对,然后计算出最优的参数 R 和 t，使得误差函数 E(R,t)最小。即：

其中n为最邻近点对的个数，pi为目标点云 P 中的一点，qi 为源点云 Q 中与pi对应的最近点，R 为旋转矩阵，t为平移向量。 如果知道正确的点对应，那么两个点集之间的相对变换（旋转、平移）就可以求得封闭解。 

ICP算法步骤和实现

ICP算法步骤：

（1）在目标点云P中取点集和初始估计 ,这里

\begin{equation}
T^0 = \left[
   \begin{matrix}
     R&t\\
     0&1
   \end{matrix}
\right]
\end{equation}

其中初始R一般选为单位矩阵，t选为零向量。

（2）找出源点云Q中的对应点集，使得 最小

（3）计算旋转矩阵R和平移矩阵t (R和t构成)，使得误差函数 E(R,t)最小(最小化这个目标函数，即求R和t。R和t的求解方法有SVD和BA)

（4）对  使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，得到新的对应点集(或)

（5）计算  与对应点集 Q 的平均距离

\begin{equation}
 dis = \frac{1}{n}\sum_{i=1}^{n}||\hat{p}_i-q_i||^2
\end{equation}

（6）如果dis小于某一给定的阈值或者大于预设的最大迭代次数，则停止迭代计算。否则用  更新  (即新的点集)，返回第 (2)步，直到满足收敛条件为止。最后的变换矩阵T满足:

\begin{equation}
 T = T^{k}\cdot T^{k-1}\cdots T^1\cdot T^0
\end{equation}

 

算法的另一种收敛条件：

\begin{equation}
  \delta=\left|\frac{E^{k+1}-E^k}{M}\right|<\epsilon(\epsilon>0)
\end{equation}

 

如何使用SVD求解R和t 

这需要注意如下最小化问题是等价的

1/2是为了容易计算R和t。

1、计算两个点云的质心(假定n=np=nq)

2、从两个点集(云)分别减去对应的质心

并记新的集合为 P' 和 Q' 。 

3、

经过计算问题转化为：(详细参见:https://blog.csdn.net/zhouyelihua/article/details/77807541)

因为左右两边都大于等于零，而且右边第一项边只和 R 相关，可以先求出R。然后利用R求解第二项。得到t。总结来说，计算过程如下。

1、计算两组质心位置,然后计算每个点的去质心坐标：

\begin{equation} 
q'_i=q_i-\mu_{q}, p'_i=p_i-\mu_{p}
\end{equation}

2、根据以下优化问题计算旋转矩阵R(这里可参考 https://blog.csdn.net/zhouyelihua/article/details/77807541)：

\begin{equation}
R^*=argmin\left(\frac{1}{2} \sum_{i=1}^{n}||q'_{i}-Rp'_{i}||^{2}\right)
\end{equation}

3、根据2的结果计算t 

\begin{equation}
t^*=\mu_q-R\mu_p
\end{equation}

那么到底如何计算R呢？利用SVD方法

引入了SVD，接下来就是求R和t。首先计算去形心点集(云)的协方差矩阵

(为什么参见：https://blog.csdn.net/zhouyelihua/article/details/77807541) 

\begin{equation}
W=\sum_{i=1}^{n}p'_i q'_i
\end{equation}

然后可以得到W的奇异值分解

如果 rank(W)=3,则 E(R,t)的最优解唯一。那么便可得到

\begin{equation}
R*=UV^{T}, t*=\mu_q-R\mu_p
\end{equation}