二维点云ICP原理推导
描述
ICP是迭代就近点算法,大部分的实现代码都是基于PCL点云库的,也就是三维点云的匹配
实际上,二维点云数据也算是常见的数据类型,比如移动机器人经常使用的单线雷达。本文就是二维点云ICP的原理推导
算法原理
二维点云数据说明
先说明单线激光雷达数据类型
d
a
t
a
=
[
r
i
θ
i
]
data=\left[ \begin{array}{c} r_{i} \\ \theta _{i} \end{array} \right]
data=[riθi]
r和θ代表每一束激光的距离和扫描角度,i代表雷达扫描一圈的每束激光。因此每帧点云数据根据极坐标变换后就是
A
=
[
x
i
y
i
]
=
[
r
i
⋅
cos
θ
r
i
⋅
sin
θ
]
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=[xiyi]=[ri⋅cosθri⋅sinθ]
现在假设我们有两帧点云A与B,我们把 A 称为标准点云,把 B 称为源点云。我们的需求是把点云 B 经过矩阵变换到点云 A,需要注意的是点云 A 和点云 B 是在同一坐标系下。
矩阵变换计算
在同一坐标系下,将某一片点云变换到另外一片点云,需要矩阵变换的知识。
假设我们需要点云在x方向平移Δx, y方向平移Δy, 点云整体旋转θ弧度
旋转矩阵R如下
R
=
[
cos
θ
sin
θ
−
sin
θ
cos
θ
]
R=\left[ \begin{array}{cc} \cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array} \right]
R=[cosθ−sinθsinθcosθ]
平移向量t如下
t
=
[
Δ
x
Δ
y
]
t\; =\; \left[ \begin{array}{c} \Delta x \\ \Delta y \end{array} \right]
t=[ΔxΔy]
则变换后的点云
A
′
A'
A′为
A
′
=
R
⋅
A
+
t
A'=R\cdot A+t
A′=R⋅A+t
ICP算法核心
ICP算法很简单,简单来说就是对b点云不停进行矩阵变换,把它变换到和a很接近就停止。专业的说,分为如下几步
- 点云归一化
- 找到两片点云中的对应点对
- 根据对应点对的信息,计算得到这次的变换矩阵
- 变换点云
- 计算当前的损失
- 是否决定继续迭代。如果损失小于设定阈值或者达到最大迭代数停止,否则返回执行第1步
1. 点云归一
点云归一化只是我的一个说法,实际并不完全等同传统的归一化操作。
这里做的操作是,取点云的中心(或者有权重时可以取重心,或者其他合理的点也是可以的)
A c e n t e r = 1 n ∑ i = 1 n A i A_{center}\; =\frac{1}{n}\sum_{i=1}^{n}{A_{i}} Acenter=n1i=1∑nAi
B c e n t e r = 1 n ∑ i = 1 n B i B_{center}\; =\frac{1}{n}\sum_{i=1}^{n}{B_{i}} Bcenter=n1i=1∑nBi
这里
A
c
e
n
t
e
r
A_{center}
Acenter和
B
c
e
n
t
e
r
B_{center}
Bcenter实际就是点云A和点云B中的平均值。
A
i
A_{i}
Ai和
B
i
B_{i}
Bi是点云中的第i个点。接下来将每个点都减去这个平均值。
a
i
=
A
i
−
A
c
e
n
t
e
r
a_{i}=\; A_{i}-A_{center}
ai=Ai−Acenter
b i = B i − B c e n t e r b_{i}=\; B_{i}-B_{center} bi=Bi−Bcenter
接下来的一部分操作是要使用归一化后的点集 a a a 和 b b b,有一部分是要使用原来的点云 A A A 和 B B B 的,请注意区分
2. 确定对应点对
确定对应点对这是一个比较有意思的话题。什么是对应点对?
正确的理解就是,点云A和点云B的这两个点,可能原来就是一个点。但实际上,当A和B还没有经过ICP时,往往这个关系是模糊的,没有人能够说出哪两个点是对应的。那该怎么办呢?好啦,ICP的核心来啦。
ICP算法的对应点对,计算欧式距离最近的被认为是对应点对,也就是ICP的C,closet
具体实施上可以这样做,经过归一化后的点集
a
a
a 和
b
b
b中,
a
i
a_{i}
ai 的对应点
b
j
b_{j}
bj 是
{
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(R,t) D(R,t)
D
(
R
,
t
)
=
1
n
∑
i
=
1
n
∣
a
i
−
R
⋅
b
i
∣
2
D\left( R,t \right)=\frac{1}{n}\sum_{i=1}^{n}{\left| a_{i}-R\cdot b_{i} \right|^{2}}
D(R,t)=n1i=1∑n∣ai−R⋅bi∣2
拆开平方项的结果就是如下
D
(
R
,
t
)
=
1
n
(
∑
i
=
1
n
∣
a
i
∣
2
+
∑
i
=
1
n
∣
R
⋅
b
i
∣
2
−
2
∑
i
=
1
n
∣
a
i
R
b
i
∣
)
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}Rb_{i} \right|} \right)
D(R,t)=n1(i=1∑n∣ai∣2+i=1∑n∣R⋅bi∣2−2i=1∑n∣aiRbi∣)
由于前两项是定值(
∣
R
⋅
b
i
∣
2
{\left| R\cdot b_{i} \right|^{2}}
∣R⋅bi∣2的值就是
(
b
i
x
)
2
+
(
b
i
y
)
2
(b_i^x)^2+(b_i^y)^2
(bix)2+(biy)2的值,是不会随着R的变化发生改变)
因此,误差函数实际上可以认为是
E
(
R
,
t
)
E\left( R,t \right)
E(R,t)
E
(
R
,
t
)
=
∑
i
=
1
n
∣
a
i
R
b
i
∣
E\left( R,t \right) =\sum_{i=1}^{n}{\left| a_{i}Rb_{i} \right|}
E(R,t)=i=1∑n∣aiRbi∣
误差函数值 E ( R , t ) E\left( R,t \right) E(R,t)越大,总的距离和越小。
在这里我好好推一下公式,说明一下这是怎么得到的吧
距离和函数为
D
(
R
,
t
)
D(R,t)
D(R,t)其实就是对所有的对应点对距离来求和。直接写成矩阵形式会更好理解
D
(
R
,
t
)
=
1
n
∑
i
=
1
n
∣
[
a
i
x
a
i
y
]
−
[
cos
θ
sin
θ
−
sin
θ
cos
θ
]
⋅
[
b
i
x
b
i
y
]
∣
2
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}}
D(R,t)=n1i=1∑n∣∣∣∣[aixaiy]−[cosθ−sinθsinθcosθ]⋅[bixbiy]∣∣∣∣2
=
1
n
∑
i
=
1
n
∣
[
a
i
x
−
cos
θ
⋅
b
i
x
−
sin
θ
⋅
b
i
y
a
i
y
+
sin
θ
⋅
b
i
x
−
cos
θ
⋅
b
i
y
]
∣
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}}
=n1i=1∑n∣∣∣∣[aix−cosθ⋅bix−sinθ⋅biyaiy+sinθ⋅bix−cosθ⋅biy]∣∣∣∣2
=
1
n
∑
i
=
1
n
[
(
a
i
x
−
cos
θ
⋅
b
i
x
−
sin
θ
⋅
b
i
y
)
2
+
(
a
i
y
+
sin
θ
⋅
b
i
x
−
cos
θ
⋅
b
i
y
)
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]}
=n1i=1∑n[(aix−cosθ⋅bix−sinθ⋅biy)2+(aiy+sinθ⋅bix−cosθ⋅biy)2]
这步推导我真的不写详细在这里,因为已经真的很简单了,推到下一步如下
=
1
n
∑
i
=
1
n
{
(
a
i
x
)
2
+
(
a
i
y
)
2
+
(
b
i
x
)
2
+
(
b
i
y
)
2
−
2
[
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
)
]
}
=\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\}}
=n1i=1∑n{(aix)2+(aiy)2+(bix)2+(biy)2−2[cosθ(aix⋅bix+aiy⋅biy)+sinθ(aix⋅biy−aiy⋅bix)]}
所以推导出误差函数如下
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)=n1i=1∑n{[cosθ(aix⋅bix+aiy⋅biy)+sinθ(aix⋅biy−aiy⋅bix)]}
这里的变量只有
θ
θ
θ ,对误差函数求导,就能得出它的极大值
σ
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)=n1i=1∑n{[−sinθ(aix⋅bix+aiy⋅biy)+cosθ(aix⋅biy−aiy⋅bix)]}=0
因此进一步推导
tan
θ
=
∑
i
=
1
n
(
a
i
x
⋅
b
i
y
−
a
i
y
⋅
b
i
x
)
∑
i
=
1
n
(
a
i
x
⋅
b
i
x
+
a
i
y
⋅
b
i
y
)
\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)}}
tanθ=∑i=1n(aix⋅bix+aiy⋅biy)∑i=1n(aix⋅biy−aiy⋅bix)
旋转弧度
θ
θ
θ的计算如下
θ
=
arctan
(
∑
i
=
1
n
(
a
i
x
⋅
b
i
y
−
a
i
y
⋅
b
i
x
)
∑
i
=
1
n
(
a
i
x
⋅
b
i
x
+
a
i
y
⋅
b
i
y
)
)
\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)
θ=arctan(∑i=1n(aix⋅bix+aiy⋅biy)∑i=1n(aix⋅biy−aiy⋅bix))
旋转矩阵 R R R 因此也能得到
平移向量
t
t
t的计算方式如下
t
=
[
Δ
x
Δ
y
]
=
[
A
c
e
n
t
e
r
x
A
c
e
n
t
e
r
y
]
−
R
[
B
c
e
n
t
e
r
x
B
c
e
n
t
e
r
y
]
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]
t=[ΔxΔy]=[AcenterxAcentery]−R[BcenterxBcentery]
解释一下也很简单,本来是可以A中心减B中心的,但是刚才已经证明了经过旋转矩阵
R
R
R 会令点对距离和更小,因此B的中心点位置也就同时经历矩阵
R
R
R 的变换
4. 变换点云
第三步我们已经求出了旋转矩阵 R R R 和平移向量 t t t,接下来就是将原点云进行矩阵变换了。注意,是原来的点云 A A A 和点云 B B B ,而不是归一化后的点云 a a a 和点云 b b b。为什么?你的操作对象一直是点云 A A A 和点云 B B B ,归一化后的 a a a 和 b b b 只是你需要的中间变量而已
对源点云
B
B
B 做变换,得到新的点云
B
′
B'
B′
B
′
=
R
⋅
B
+
t
B'\; =\; R\cdot B+t
B′=R⋅B+t
5. 计算损失
将最新变换出的矩阵 B ′ B' B′,与你的目标点云 A A A 进行计算。可以自行设计属于自己的损失函数,来表示两片点云究竟匹配的怎么样。
我在这里设计的损失函数仍然是对应点对(最近点)的距离之和
L
o
s
s
=
D
(
R
,
t
)
=
1
n
∑
i
=
1
n
∣
A
i
−
B
i
’
∣
2
Loss= D\left( R,t \right)=\frac{1}{n}\sum_{i=1}^{n}{\left| A_{i}- B^’_{i} \right|^{2}}
Loss=D(R,t)=n1i=1∑n∣∣Ai−Bi’∣∣2
6. 是否继续迭代
接下来就要看这个计算出的 L o s s Loss Loss 值能否满足你的要求了。
迭代终止条件的设计其实比较自由。但我的经验告诉我,比较常见的是以下几种
- L o s s Loss Loss < L o s s m i n Loss_{min} Lossmin : 当前迭代得到的损失值小于了设定的最小阈值了
- i t e r a t i o n iteration iteration < i t e r a t i o n m a x iteration_{max} iterationmax : 迭代次数超过了最大迭代次数
- Δ L o s s ΔLoss ΔLoss< Δ L o s s m i n ΔLoss_{min} ΔLossmin : 损失值的变化小于了设定的最小变化值
分别代表的意义也是明显的:1)当前已经匹配的很好了,别迭代了;2) 已经匹配了好多次了,饭都凉了;3)这一次都没提升多少,你还继续匹配干嘛
结束语
这篇文章我真的写了好久了,公式都是我生推的,分析都是生写的,给个赞吧哈哈,谢谢