ICP算法

• 1.ICP算法（迭代最近点算法）
• • 1.ICP算法的求解
  • 2.数学证明

1.ICP算法（迭代最近点算法）

ICP(迭代最近点算法)是一种古老而又经典的点云匹配算法，其思路也很清晰。点云配准就是求解两堆点云之间的变换关系——也就是旋转关系 R R R 和平移关系 t t t。

ICP算法的思路就是：找到两组点云集合中距离最近的点对，根据估计的变换关系（ R R R 和 t t t）来计算距离最近点对经过变换之后的误差，进过不断的迭代直至误差小于某一阈值或者达到迭代次数来确定最终的变换关系。

ICP算法的数学描述：

给定两个点云集合：

X = ( x 1 , x 2 , x 3 . . . x n ) X =(x_{1},x_{2},x_{3}...x_{n}) X=(x1​,x2​,x3​...xn​) P = ( p 1 , p 2 , p 3 . . . p m ) P=(p_{1},p_{2},p_{3}...p_{m}) P=(p1​,p2​,p3​...pm​)

求解 R R R 和 t t t，使下式最小

E ( R , t ) = 1 n ∑ i = 1 n ∣ ∣ x i − ( R p i + t ) ∣ ∣ 2 E(R,t) =\frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}-(Rp_{i}+t)||^{2} E(R,t)=n1​i=1∑n​∣∣xi​−(Rpi​+t)∣∣2

1.ICP算法的求解

1.1 已知对应点的情况

如果已知两个点云中点的对应关系，那么ICP算法的求解过程将非常简单，这个在视觉slam中较为常用，因为我们可以根据特种匹配的方式找出空间点的对应关系。

具体方法为：

1. 首先，计算两组点云的质心

   u x = 1 n ∑ i = 1 n ∣ ∣ x i ∣ ∣ 2 u_{x} =\frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}||^{2} ux​=n1​i=1∑n​∣∣xi​∣∣2

   u p = 1 m ∑ i = 1 m ∣ ∣ p i ∣ ∣ 2 u_{p} =\frac{1}{m}\displaystyle\sum_{i=1}^{m}||p_{i}||^{2} up​=m1​i=1∑m​∣∣pi​∣∣2

2. 计算两组点云中的点以质心为原点的坐标

   X ′ = ( x i − u x ) = ( x i ′ ) X' =(x_{i}-u_{x})=(x_{i}') X′=(xi​−ux​)=(xi′​) P ′ = ( p i − u p ) = ( p i ′ ) P'=(p_{i}-u_{p})=(p_{i}') P′=(pi​−up​)=(pi′​)

3. 计算 w w w并对其进行SVD分解

   w = 1 n ∑ i = 1 n x i ′ p i ′ T = U [ δ 1 0 0 0 δ 2 0 0 0 δ 3 ] V T w= \frac{1}{n}\displaystyle\sum_{i=1}^{n}x_{i}'p_{i}'^T=U\left[ \begin{matrix} \delta_{1} & 0 &0\\ 0 & \delta_{2} & 0 \\ 0& 0& \delta_{3} \end{matrix}\right]V^T w=n1​i=1∑n​xi′​pi′T​=U⎣⎡​δ1​00​0δ2​0​00δ3​​⎦⎤​VT

4. 则两组点云之间的变换关系为（即ICP算法的解）：

   R = V U T ， t = u x − R u p R = VU^T ，t = u_{x}-Ru_{p} R=VUT，t=ux​−Rup​

1.2 未知对应点的情况

已知对应点的情况下，我们可以一次性计算出点云之间的变换关系。但是在激光slam中，我们并不知道两组点云之间的对应关系。因此也就不能通过一次计算就求解出点云之间的变换关系。这时候我们的策略就是使用迭代的方式。

具体方法为：

1. 寻找两组点云中距离最近的点对。
2. 根据找到的距离最近点对，来求解两组点云之间的位姿关系。
3. 根据求解的位姿关系对点云进行变换，并计算误差。
4. 若误差没有达到要求，则重复1，2，3步直至误差满足要求或达到最大迭代次数。

ICP算法很简单，但是其思想却很巧妙。为什么这么说呢？因为ICP算法要求解的是两组点云之间的变换关系，要求解这种变换关系需要直到两组点云中点的对应关系。可是我们并不知道这种点的对应关系。那怎么办？ICP算法的策略是，反正我不知道两组点云之间点的对应关系，那就认为距离最近的点是相互匹配的，然后以此来计算两组点云之间的变换关系。在得到变换关系之后，对点云进行变换并计算误差。然后再次选取距离最近的点作为匹配点并求解变换关系，重复该过程直至误差满足要求。

这个过程就好比，我需要有条件A才能计算B。但是我并没有A，可我还是想求B（这不就是想空手套白狼嘛）。那怎么办?既然我们没有A，那就找个东西代替A（记为 A ′ A' A′），用这个代替品来求解B。这时候就算的B肯定的不准确的。没关系，不准确总比没有好。我们假设B是准确的并用它来对原始数据进行处理。然后在处理过的数据中再次寻找 A ′ A' A′来计算B，重复该过程直至满足条件。仔细想想这就是 A和B之间固定一个求解另一个的过程。通过这样的过程来一步步求解出满足条件的B。

2.数学证明

通过上面的分析可以看出，已知对应点的情况是未知对应点情况的一部分。接下来我们来证明一下已知对应点的求解方法。

误差函数可以写成如下形式：

E ( R , t ) = 1 n ∑ i = 1 n ∣ ∣ x i − ( R p i + t ) ∣ ∣ 2 = 1 n ∑ i = 1 n ∣ ∣ x i − ( R p i + t ) − u x + u x + R u p − R u p ∣ ∣ 2 = 1 n ∑ i = 1 n ∣ ∣ x i − u x − R ( p i − u p ) + ( u x − R u p − t ) ∣ ∣ 2 E(R,t) =\frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}-(Rp_{i}+t)||^{2}\\ = \frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}-(Rp_{i}+t)-u_{x}+u_{x}+Ru_{p}-Ru_{p}||^{2}\\ = \frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}-u_{x}-R(p_{i}-u_{p})+(u_{x}-Ru_{p}-t)||^{2} E(R,t)=n1​i=1∑n​∣∣xi​−(Rpi​+t)∣∣2=n1​i=1∑n​∣∣xi​−(Rpi​+t)−ux​+ux​+Rup​−Rup​∣∣2=n1​i=1∑n​∣∣xi​−ux​−R(pi​−up​)+(ux​−Rup​−t)∣∣2

然后对误差函数展开：

E ( R , t ) = 1 n ∑ i = 1 n ∣ ∣ x i − u x − R ( p i − u p ) ∣ ∣ 2 + ∣ ∣ u x − R u p − t ∣ ∣ 2 + 2 ( x i − u x − R ( p i − u p ) ) T ( u x − R u p − t ) E(R,t)= \frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}-u_{x}-R(p_{i}-u_{p})||^2+||u_{x}-Ru_{p}-t||^{2}\\+2(x_{i}-u_{x}-R(p_{i}-u_{p}))^T(u_{x}-Ru_{p}-t) E(R,t)=n1​i=1∑n​∣∣xi​−ux​−R(pi​−up​)∣∣2+∣∣ux​−Rup​−t∣∣2+2(xi​−ux​−R(pi​−up​))T(ux​−Rup​−t)

因为交叉项在进行求和之后为0，所以误差函数可以表示为

E ( R , t ) = 1 n ∑ i = 1 n ∣ ∣ x i − u x − R ( p i − u p ) ∣ ∣ 2 + ∣ ∣ u x − R u p − t ∣ ∣ 2 E(R,t)= \frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}-u_{x}-R(p_{i}-u_{p})||^2+||u_{x}-Ru_{p}-t||^{2}\\ E(R,t)=n1​i=1∑n​∣∣xi​−ux​−R(pi​−up​)∣∣2+∣∣ux​−Rup​−t∣∣2

其中 ∣ ∣ x i − u x − R ( p i − u p ) ∣ ∣ 2 ||x_{i}-u_{x}-R(p_{i}-u_{p})||^2 ∣∣xi​−ux​−R(pi​−up​)∣∣2只与 R R R有关。当已知 R R R时可以通过 u x − R u p − t u_{x}-Ru_{p}-t ux​−Rup​−t求解得到 t t t。

所以，误差函数可以表示为

E ( R , t ) = 1 n ∑ i = 1 n ∣ ∣ x i − u x − R ( p i − u p ) ∣ ∣ 2 E(R,t)= \frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}-u_{x}-R(p_{i}-u_{p})||^2 E(R,t)=n1​i=1∑n​∣∣xi​−ux​−R(pi​−up​)∣∣2

然后在对其进行展开，则有

E ( R , t ) = 1 n ∑ i = 1 n ∣ ∣ x i − u x − R ( p i − u p ) ∣ ∣ 2 = 1 n ∑ i = 1 n ∣ ∣ x i ′ − u x − R p i ′ ∣ ∣ 2 = 1 n ∑ i = 1 n x i ′ T x i ′ + p i ′ T R T R p i ′ − 2 x i ′ T R p i ′ = 1 n ∑ i = 1 n − 2 x i ′ T R p i ′ E(R,t)= \frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}-u_{x}-R(p_{i}-u_{p})||^2 \\= \frac{1}{n}\displaystyle\sum_{i=1}^{n}||x_{i}'-u_{x}-Rp_{i}'||^2 \\= \frac{1}{n}\displaystyle\sum_{i=1}^{n}x_{i}'^Tx_{i}'+p_{i}'^TR^TRp_{i}'-2x_{i}'^TRp_{i}' \\=\frac{1}{n}\displaystyle\sum_{i=1}^{n}-2x_{i}'^TRp_{i}' E(R,t)=n1​i=1∑n​∣∣xi​−ux​−R(pi​−up​)∣∣2=n1​i=1∑n​∣∣xi′​−ux​−Rpi′​∣∣2=n1​i=1∑n​xi′T​xi′​+pi′T​RTRpi′​−2xi′T​Rpi′​=n1​i=1∑n​−2xi′T​Rpi′​

其中

∑ i = 1 n x i ′ T R p i ′ = ∑ i = 1 n t r a c e ( R x i ′ p i ′ T ) = t r a c e ( R H ) \displaystyle\sum_{i=1}^{n}x_{i}'^TRp_{i}' =\displaystyle\sum_{i=1}^{n}trace(Rx_{i}'p_{i}'^T)=trace(RH) i=1∑n​xi′T​Rpi′​=i=1∑n​trace(Rxi′​pi′T​)=trace(RH)

此时，求解误差函数的最小值即为求解 t r a c e ( R H ) trace(RH) trace(RH)的最大值。

在此引入定理：

假设矩阵A为正定对称矩阵，则对于任意的正交矩阵B，都有： t r ( A ) > = t r ( B ) tr(A)>=tr(B) tr(A)>=tr(B)

根据此定理，则有

由于 B B B是任意的正交矩阵，所以 B X BX BX可以表示任意的正交矩阵，所以上面这个不等式表达的意思就是指： X H XH XH的迹大于任意正交矩阵与 H H H内积的迹。所以 t r a c e ( X H ) trace(XH) trace(XH)为最大值。

由此 t r a c e ( R H ) trace(RH) trace(RH)取最大值时， R R R 的值为

R = V U T R=VU^T R=VUT