ICP算法

ICP算法介绍

ICP算法是基于EM思想的方法，采用交替迭代法优化得到最优值。即ICP分为两步迭代优化，优化点云匹配及优化运动估计。

点云匹配是将两帧点云数据在同一个坐标系下，一帧数据中的点找到另一帧数据最近的点，就作为一对匹配点。

运动估计就是根据得到得两帧点云得匹配情况，构建最小二乘方程，求解。假设已知两帧点云图之间的匹配关系，那么求解两帧点云之间的位姿就是求最小二乘方程的解，即以下方程的解$R$R与$t$t。

其中，$R$R是3*3矩阵，$t$t是3*1向量。$N_p$Np​表示点数。

求解得到运动估计，ICP采用SVD分解求的运动估计得最优值。

除了使用逐步迭代法(最速/牛顿/LM等)求解，ICP算法采用SVD分解的方法，求得运动估计最优解。在时间占用上比使用四元数计算要大，但是比迭代法小很多，但是在精度上提高了很多。列出原文表格：

运动估计算法介绍：

已知点云的匹配关系后，可以根据匹配点的两帧位置的不同，估算出机器人的运动。

假设如果我们已经知道两帧点云图之间的匹配关系，那么求解两帧点云之间的位姿就是构建最小二乘目标函数，求解最优解R与t。这个目标函数实际上就是所有对应点之间的欧式距离的平方和。

其中，$R$R是3*3矩阵，$t$t是3*1向量。$N_p$Np​表示点数。

求解原理：利用SVD先求解旋转矩阵$R$R，再求解偏移$t$t向量

给定两个点云集合:
$p^t=\left\{p_1^t,p_2^t,\cdots p_n^t\right\}$pt={p1t​,p2t​,⋯pnt​}

$p^s=\left\{p_1^s,p_2^s,\cdots p_n^s\right\}$ps={p1s​,p2s​,⋯pns​}

求解$R$R和$t$t，使得目标方程(*)最小。

先求$R$R，再求$t$t:
$p^t=Rp^s-t$pt=Rps−t

(1):
$p_i^t=Rp_i^s-t$pit​=Rpis​−t

令：$c^t, c^s$ct,cs为两帧点云中心:
$c^t=\frac{1}{N_p}\sum_{i=1}^{N_p}p_i^t$ct=Np​1​i=1∑Np​​pit​

$c^s=\frac{1}{N_p}\sum_{i=1}^{N_p}p_i^s$cs=Np​1​i=1∑Np​​pis​
则：(2):
$c^t=Rc^s-t$ct=Rcs−t

令：
$u^t=\left\{p_i^t-u_t\right\}$ut={pit​−ut​}

$u^s=\left\{p_i^s-u_s\right\}$us={pis​−us​}

则令（2）减去（1）可得：
$u^t=Ru^s$ut=Rus

即可通过最小化以下目标方程: 得$R$R。

其中$R$R为旋转阵，满足$R^TR=I$RTR=I，则前两项与$R$R无关，问题转化为：

令：

则问题化为：
$R^\ast=argmax\ tr(RH)$R∗=argmax tr(RH)

由定理：
$tr\left(AA^T\right)\geq\ tr\left(BAA^T\right)，BB^T=I$tr(AAT)≥ tr(BAAT)，BBT=I

可知，当$R$R阵$RH$RH使可以化为$AA^T$AAT时，$tr(RH)$tr(RH)最大。
将$H$H进行SVD分解：
$H=U\Sigma V^T$H=UΣVT
$\Sigma$Σ为对角阵，$U$U及$V$V为正交阵。
令：
$R^\ast H=R^\ast U\Sigma V^T=AA^T$R∗H=R∗UΣVT=AAT

$R^\ast=VU^T，A=VΣ^\frac12$R∗=VUT，A=VΣ21​

达到最优值。

即令：

则ICP的解为：
$R=UV^T$R=UVT

$t=c^t-Rc^s$t=ct−Rcs

ICP算法流程

该部分内容来自：

简要介绍一下点集到点集ICP配准的算法流程：

1)ICP算法核心是最小化一个目标函数：

就是一对对应点，总共有Np对对应点。这个目标函数实际上就是所有对应点之间的欧式距离的平方和。

2)寻找对应点。可是，我们现在并不知道有哪些对应点。因此，我们在有初值的情况下，假设用初始的旋转平移矩阵对source cloud进行变换，得到的一个变换后的点云。然后将这个变换后的点云与target
cloud进行比较，只要两个点云中存在距离小于一定阈值（ICP中的一个参数），我们就认为这两个点就是对应点。这也是"最邻近点"这个说法的来源。

3)R、T优化。有了对应点之后，我们就可以用对应点对旋转R与平移T进行估计。这里R和T中只有6个自由度，而我们的对应点数量是庞大的（存在多余观测值）。因此，我们可以采用最小二乘等方法求解最优的旋转平移矩阵。一个数值优化问题，这里就不详细讲了。

4)迭代。我们优化得到了一个新的R与T，导致了一些点转换后的位置发生变化，一些最邻近点对也相应的发生了变化。因此，我们又回到了步骤2)中的寻找最邻近点方法。2)3)步骤不停迭代进行，直到满足一些迭代终止条件，如R、T的变化量小于一定值，或者上述目标函数的变化小于一定值，或者邻近点对不再变化等。（ICP算法中的一个参数）

算法大致流程就是上面这样。这里的优化过程是一个贪心的策略。首先固定R跟T利用最邻近算法找到最优的点对，然后固定最优的点对来优化R和T，依次反复迭代进行。这两个步骤都使得目标函数值下降，所以ICP算法总是收敛的，这也就是原文中收敛性的证明过程。这种优化思想与K均值聚类的优化思想非常相似，固定类中心优化每个点的类别，固定每个点的类别优化类中心。

参考连接

https://www.jianshu.com/p/dd4b49650d76
https://blog.****.net/qq_27251141/article/details/88735285
https://blog.****.net/zhouyelihua/article/details/77807541