二维点云ICP原理推导

二维点云ICP原理推导

描述

ICP是迭代就近点算法，大部分的实现代码都是基于PCL点云库的，也就是三维点云的匹配
实际上，二维点云数据也算是常见的数据类型，比如移动机器人经常使用的单线雷达。本文就是二维点云ICP的原理推导

算法原理

二维点云数据说明

先说明单线激光雷达数据类型
$$data=\left[\begin{array}{c}{r}_i\\ {\theta }_i\end{array}\right]$$
r和θ代表每一束激光的距离和扫描角度，i代表雷达扫描一圈的每束激光。因此每帧点云数据根据极坐标变换后就是
$$A=\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]=\left[\begin{array}{c}{r}_i\cdot \cos \theta \\ r_i\cdot \sin \theta \end{array}\right]$$
现在假设我们有两帧点云A与B，我们把 A 称为标准点云，把 B 称为源点云。我们的需求是把点云 B 经过矩阵变换到点云 A，需要注意的是点云 A 和点云 B 是在同一坐标系下。

矩阵变换计算

在同一坐标系下，将某一片点云变换到另外一片点云，需要矩阵变换的知识。

假设我们需要点云在x方向平移Δx， y方向平移Δy， 点云整体旋转θ弧度

旋转矩阵R如下
$$R=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]$$
平移向量t如下
$$t=\left[\begin{array}{c}\Delta x\\ \Delta y\end{array}\right]$$

则变换后的点云$A^{\prime }$为
$$A^{\prime }=R\cdot A+t$$

ICP算法核心

ICP算法很简单，简单来说就是对b点云不停进行矩阵变换，把它变换到和a很接近就停止。专业的说，分为如下几步

1. 点云归一化
2. 找到两片点云中的对应点对
3. 根据对应点对的信息，计算得到这次的变换矩阵
4. 变换点云
5. 计算当前的损失
6. 是否决定继续迭代。如果损失小于设定阈值或者达到最大迭代数停止，否则返回执行第1步

1. 点云归一

点云归一化只是我的一个说法，实际并不完全等同传统的归一化操作。
这里做的操作是，取点云的中心（或者有权重时可以取重心，或者其他合理的点也是可以的）

$$A_{center}=\frac{1}{n}\sum _{i=1}^n{A}_i$$

$$B_{center}=\frac{1}{n}\sum _{i=1}^n{B}_i$$

这里$A_{center}$和$B_{center}$实际就是点云A和点云B中的平均值。$A_i$和$B_i$是点云中的第i个点。接下来将每个点都减去这个平均值。
$$a_i=A_i-A_{center}$$

$$b_i=B_i-B_{center}$$

接下来的一部分操作是要使用归一化后的点集 $a$ 和 $b$，有一部分是要使用原来的点云 $A$ 和 $B$ 的，请注意区分

2. 确定对应点对

确定对应点对这是一个比较有意思的话题。什么是对应点对？

正确的理解就是，点云A和点云B的这两个点，可能原来就是一个点。但实际上，当A和B还没有经过ICP时，往往这个关系是模糊的，没有人能够说出哪两个点是对应的。那该怎么办呢？好啦，ICP的核心来啦。

ICP算法的对应点对，计算欧式距离最近的被认为是对应点对，也就是ICP的C，closet

具体实施上可以这样做，经过归一化后的点集 $a$ 和 $b$中，$a_i$ 的对应点 $b_j$ 是
{ a i , b j }    = a r g min ⁡ j { ( a i x − b j x ) 2 + ( a i y − b j y ) 2 } \left\{ a_{i},b_{j} \right\}\; =arg\min _{j}\left\{ \sqrt{\left( a_{i}^{x}-b_{j}^{x} \right)^{2}+\left( a_{i}^{y}-b_{j}^{y} \right)^{2}} \right\} {ai​,bj​}=argjmin​{(aix​−bjx​)2+(aiy​−bjy​)2 ​}

在PCL库的ICP算法过程中，有一个很重要的步骤就是对应关系去除。原因是数据是有噪声的，因此要去除对配准有影响的错误点对。

二维点云的数据量是小的，那么请你结合自己的需求来完成去噪这一步。这一步是必要的。

3. 变换矩阵计算

先明确到了这一步我们拥有的数据和要得到的东西。

我们有的是很多对应的点对，我们要求的是一个变换矩阵，使用这个矩阵完成变换后，这些对应的点对的距离和是最小的（意思就是说，这个矩阵至少是当前最优的，使用这个矩阵相比于其他任何变换矩阵，能让牛郎们和织女们的距离和最小）。那么我们就来进行公式推导吧。（数学老师说的没错，数学真的很重要）

设置对应点对的距离和函数为$D(R,t)$

$$D\left(R,t\right)=\frac{1}{n}\sum _{i=1}^n{\left|a_i-R\cdot b_i\right|}^2$$
拆开平方项的结果就是如下
$$D\left(R,t\right)=\frac{1}{n}\left(\sum _{i=1}^n{\left|a_i\right|}^2+\sum _{i=1}^n{\left|R\cdot b_i\right|}^2-2\sum _{i=1}^n\left|a_{i}R{b}_i\right|\right)$$
由于前两项是定值（${\left|R\cdot b_i\right|}^2$的值就是$(b_i^x{)}^2+(b_i^y{)}^2$的值，是不会随着R的变化发生改变）

因此，误差函数实际上可以认为是$E\left(R,t\right)$
$$E\left(R,t\right)=\sum _{i=1}^n\left|a_{i}R{b}_i\right|$$

误差函数值$E\left(R,t\right)$越大，总的距离和越小。

在这里我好好推一下公式，说明一下这是怎么得到的吧
距离和函数为$D(R,t)$其实就是对所有的对应点对距离来求和。直接写成矩阵形式会更好理解
$$D\left(R,t\right)=\frac{1}{n}\sum _{i=1}^n{\left|\left[\begin{array}{c}{a}_i^x\\ a_i^y\end{array}\right]-\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]\cdot \left[\begin{array}{c}{b}_i^x\\ b_i^y\end{array}\right]\right|}^2$$
$$=\frac{1}{n}\sum _{i=1}^n{\left|\left[\begin{array}{c}{a}_i^x-\cos \theta \cdot b_i^x-\sin \theta \cdot b_i^y\\ a_i^y+\sin \theta \cdot b_i^x-\cos \theta \cdot b_i^y\end{array}\right]\right|}^2$$
$$=\frac{1}{n}\sum _{i=1}^n\left[{\left(a_i^x-\cos \theta \cdot b_i^x-\sin \theta \cdot b_i^y\right)}^2+{\left(a_i^y+\sin \theta \cdot b_i^x-\cos \theta \cdot b_i^y\right)}^2\right]$$
这步推导我真的不写详细在这里，因为已经真的很简单了，推到下一步如下
$$=\frac{1}{n}\sum _{i=1}^n\left\{{\left(a_i^x\right)}^2+{\left(a_i^y\right)}^2+{\left(b_i^x\right)}^2+{\left(b_i^y\right)}^2-2\left[\cos \theta \left(a_i^x\cdot b_i^x+a_i^y\cdot b_i^y\right)+\sin \theta \left(a_i^x\cdot b_i^y-a_i^y\cdot b_i^x\right)\right]\right\}$$

所以推导出误差函数如下
E ( R , t ) = 1 n ∑ i = 1 n { [ cos ⁡ θ ( a i x ⋅ b i x + a i y ⋅ b i y ) + sin ⁡ θ ( a i x ⋅ b i y − a i y ⋅ b i x ) ] } E\left( R,t \right)=\frac{1}{n}\sum_{i=1}^{n}{\left\{ \left[ \cos \theta \left( a_{i}^{x}\cdot b_{i}^{x}+a_{i}^{y}\cdot b_{i}^{y} \right)+\sin \theta \left( a_{i}^{x}\cdot b_{i}^{y}-a_{i}^{y}\cdot b_{i}^{x} \right) \right] \right\}} E(R,t)=n1​i=1∑n​{[cosθ(aix​⋅bix​+aiy​⋅biy​)+sinθ(aix​⋅biy​−aiy​⋅bix​)]}

这里的变量只有 $\theta$ ，对误差函数求导，就能得出它的极大值
σ E ( R , t ) σ θ = 1 n ∑ i = 1 n { [ − sin ⁡ θ ( a i x ⋅ b i x + a i y ⋅ b i y ) + cos ⁡ θ ( a i x ⋅ b i y − a i y ⋅ b i x ) ] } = 0 \frac{\sigma E\left( R,t \right)}{\sigma \theta }=\frac{1}{n}\sum_{i=1}^{n}{\left\{ \left[ -\sin \theta \left( a_{i}^{x}\cdot b_{i}^{x}+a_{i}^{y}\cdot b_{i}^{y} \right)+\cos \theta \left( a_{i}^{x}\cdot b_{i}^{y}-a_{i}^{y}\cdot b_{i}^{x} \right) \right] \right\}}=0 σθσE(R,t)​=n1​i=1∑n​{[−sinθ(aix​⋅bix​+aiy​⋅biy​)+cosθ(aix​⋅biy​−aiy​⋅bix​)]}=0
因此进一步推导
$$\tan \theta =\frac{{\sum }_{i=1}^n\left(a_i^x\cdot b_i^y-a_i^y\cdot b_i^x\right)}{{\sum }_{i=1}^n\left(a_i^x\cdot b_i^x+a_i^y\cdot b_i^y\right)}$$

旋转弧度$\theta$的计算如下
$$\theta =\arctan \left(\frac{{\sum }_{i=1}^n\left(a_i^x\cdot b_i^y-a_i^y\cdot b_i^x\right)}{{\sum }_{i=1}^n\left(a_i^x\cdot b_i^x+a_i^y\cdot b_i^y\right)}\right)$$

旋转矩阵 $R$ 因此也能得到

平移向量$t$的计算方式如下
$$t=\left[\begin{array}{c}\Delta x\\ \Delta y\end{array}\right]=\left[\begin{array}{c}{A}_{center}^x\\ A_{center}^y\end{array}\right]-R\left[\begin{array}{c}{B}_{center}^x\\ B_{center}^y\end{array}\right]$$
解释一下也很简单，本来是可以A中心减B中心的，但是刚才已经证明了经过旋转矩阵 $R$ 会令点对距离和更小，因此B的中心点位置也就同时经历矩阵 $R$ 的变换

4. 变换点云

第三步我们已经求出了旋转矩阵 $R$ 和平移向量 $t$，接下来就是将原点云进行矩阵变换了。注意，是原来的点云 $A$ 和点云 $B$ ，而不是归一化后的点云 $a$ 和点云 $b$。为什么？你的操作对象一直是点云 $A$ 和点云 $B$ ，归一化后的 $a$ 和 $b$ 只是你需要的中间变量而已

对源点云 $B$ 做变换，得到新的点云 $B^{\prime }$
$$B^{\prime }=R\cdot B+t$$

5. 计算损失

将最新变换出的矩阵 $B^{\prime }$，与你的目标点云 $A$ 进行计算。可以自行设计属于自己的损失函数，来表示两片点云究竟匹配的怎么样。

我在这里设计的损失函数仍然是对应点对（最近点）的距离之和
$$Loss=D\left(R,t\right)=\frac{1}{n}\sum _{i=1}^n{\left|A_i-B_i^{’}\right|}^2$$

6. 是否继续迭代

接下来就要看这个计算出的 $Loss$ 值能否满足你的要求了。

迭代终止条件的设计其实比较自由。但我的经验告诉我，比较常见的是以下几种

1. $Loss$ < $Los{s}_{min}$ : 当前迭代得到的损失值小于了设定的最小阈值了
2. $iteration$ < $iteratio{n}_{max}$ : 迭代次数超过了最大迭代次数
3. $\Delta Loss$<$\Delta Los{s}_{min}$ : 损失值的变化小于了设定的最小变化值

分别代表的意义也是明显的：1）当前已经匹配的很好了，别迭代了；2） 已经匹配了好多次了，饭都凉了；3）这一次都没提升多少，你还继续匹配干嘛

结束语

这篇文章我真的写了好久了，公式都是我生推的，分析都是生写的，给个赞吧哈哈，谢谢