Generalized-ICP
文章目录
Abstract
这篇文章的提供一种新方法，可以将point-to-point icp 与point-to-plane icp整合起来。新方法对于错误匹配更加鲁棒，使得maximum match distance参数(详见下文)的调节更加容易。除了性能上的提升以外，新方法允许更加expressive的 probabilistic model 整合进ICP框架里来。
Introduction
ScanMatching
整理了ICP的各种变体，回顾了经典ICP方法。
强调了 max_correspondence参数的设置，该参数是为了防止两片点云之间不存在overlap，所以要限制查找最近点的距离。在使用时，如果设置的太小， 算法会变得short sighted，容易导致bad convergence。设的太大，则容易出现错误匹配，影响算法结果。
Generalized ICP
本文方法的核心思想是如何从概率的角度去看待和推导出ICP算法的目标函数。
假设有两个匹配好的点集， A = { a i } i = 1 , 2... N , B = { b i } i = 1 , 2... N , 且 a i 和 b i 是 对 应 点 A=\{a_i\}_{i=1,2...N},B=\{b_i\}_{i=1,2...N},且a_i和b_i是对应点 A={ai}i=1,2...N,B={bi}i=1,2...N,且ai和bi是对应点（A为source，B为target）
再假设两个点云中的每个点，都是服从高斯分布的，即：
a
i
∼
N
(
a
i
^
,
C
i
A
)
b
i
∼
N
(
b
i
^
,
C
i
B
)
a_i\sim N(\hat{a_i},C_i^{A})\\ b_i\sim N(\hat{b_i},C_i^{B})\\
ai∼N(ai^,CiA)bi∼N(bi^,CiB)
（个人理解，由于测量等环节的误差，每个点的位置的测量值实际上是和真值（
a
i
^
,
b
i
^
即
是
真
值
\hat{a_i},\hat{b_i}即是真值
ai^,bi^即是真值）有偏差的，我们可以合理假设他们的分布都是高斯分布）
对于
a
i
^
,
b
i
^
\hat{a_i},\hat{b_i}
ai^,bi^有：
b
^
i
=
T
∗
a
^
\hat{b}_i=T^*\hat{a}
b^i=T∗a^
T
∗
T^*
T∗是理想中的correct rigid transform。
定义残差 d i ( T ) = b i − T a i d_i^{(T)}=b_i-Ta_i di(T)=bi−Tai
因为
a
i
,
b
i
a_i,b_i
ai,bi都被我们假设为独立的、服从高斯分布的随机变量，所以有：
d
i
T
∗
∼
N
(
0
,
C
i
B
+
(
T
∗
)
C
i
A
(
T
∗
)
T
)
d_i^{T*}\sim N(0,C_i^{B}+(T^*)C_i^{A}(T^*)^T)
diT∗∼N(0,CiB+(T∗)CiA(T∗)T)
接下来就是这篇文章的重点，
T
T
T可以被看作
d
i
T
d_i^T
diT的概率分布中待估计的分布参数，借助最大似然估计（MLE）的思想，有：
T
=
arg
max
T
∏
i
p
(
d
i
T
)
T=\mathop{\arg\max}_T\prod_ip(d_i^{T})
T=argmaxTi∏p(diT)
上式可以化简为：
T
=
arg
min
T
∑
i
d
i
(
T
)
T
(
C
i
B
+
T
C
i
A
T
T
)
−
1
d
i
(
T
)
T=\mathop{\arg\min}_T\sum_id_i^{(T)^{T}} (C_i^B+TC_i^AT^T)^{-1}d_i^{(T)}
T=argminTi∑di(T)T(CiB+TCiATT)−1di(T)
这里有点疑问，我们稍微推导下：
多元正态分布(
K
K
K维随机变量)的概率密度函数：
f
μ
,
Σ
(
x
)
=
1
(
2
π
)
K
2
⋅
1
∣
Σ
∣
1
2
⋅
e
−
1
2
(
x
−
μ
)
T
Σ
−
1
(
x
−
μ
)
f_{\mu, \Sigma}(x)=\frac{1}{(2 \pi)^{\frac{K}{2}}} \cdot \frac{1}{|\Sigma|^{\frac{1}{2}}} \cdot e^{-\frac{1}{2}(x-\mu)^{T} \Sigma^{-1}(x-\mu)}
fμ,Σ(x)=(2π)2K1⋅∣Σ∣211⋅e−21(x−μ)TΣ−1(x−μ)
对于
N
N
N个样本点
x
1
,
x
2
.
.
.
x
n
x_1,x_2...x_n
x1,x2...xn，其似然函数：
L
(
μ
,
Σ
)
=
f
μ
,
Σ
(
x
1
)
f
μ
,
Σ
(
x
2
)
…
f
μ
,
Σ
(
x
N
)
=
(
2
π
)
−
K
N
2
⋅
∣
Σ
∣
−
N
2
⋅
e
−
1
2
∑
n
=
1
N
(
x
n
−
μ
)
T
Σ
−
1
(
x
n
−
μ
)
\begin{array}{c} L(\mu, \Sigma)=f_{\mu, \Sigma}\left(x^{1}\right) f_{\mu, \Sigma}\left(x^{2}\right) \ldots f_{\mu, \Sigma}\left(x^{N}\right) \\ =(2 \pi)^{-\frac{K N}{2}} \cdot|\Sigma|^{-\frac{N}{2}} \cdot e^{-\frac{1}{2} \sum_{n=1}^{N}\left(x^{n}-\mu\right)^{T} \Sigma^{-1}\left(x^{n}-\mu\right)} \end{array}
L(μ,Σ)=fμ,Σ(x1)fμ,Σ(x2)…fμ,Σ(xN)=(2π)−2KN⋅∣Σ∣−2N⋅e−21∑n=1N(xn−μ)TΣ−1(xn−μ)
取对数：
ln
L
(
μ
,
Σ
)
=
−
K
N
2
ln
(
2
π
)
−
N
2
ln
∣
Σ
∣
−
1
2
∑
n
=
1
N
(
x
n
−
μ
)
T
Σ
−
1
(
x
n
−
μ
)
=
C
−
N
2
ln
∣
Σ
∣
−
1
2
∑
n
=
1
N
(
x
n
−
μ
)
T
Σ
−
1
(
x
n
−
μ
)
\begin{aligned} \ln L(\mu, \Sigma)=-\frac{K N}{2} \ln (2 \pi)-\frac{N}{2} \ln |\Sigma|-\frac{1}{2} \sum_{n=1}^{N}\left(x^{n}-\mu\right)^{T} \Sigma^{-1}\left(x^{n}-\mu\right) \\ &=C-\frac{N}{2} \ln |\Sigma|-\frac{1}{2} \sum_{n=1}^{N}\left(x^{n}-\mu\right)^{T} \Sigma^{-1}\left(x^{n}-\mu\right) \end{aligned}
lnL(μ,Σ)=−2KNln(2π)−2Nln∣Σ∣−21n=1∑N(xn−μ)TΣ−1(xn−μ)=C−2Nln∣Σ∣−21n=1∑N(xn−μ)TΣ−1(xn−μ)
当正态分布的协方差矩阵的行列式为常数时，只需要优化最后一项就可以了。最后一项的二次型又被称作马哈拉诺比斯距离（马氏距离），极大似然估计等价于最小化样本点与均值之间的马氏距离。更详细的内容可以参考 高翔《视觉SLAM14讲》6.1 状态估计问题 。
但是本文中协方差矩阵为 Σ = C i B + ( T ∗ ) C i A ( T ∗ ) T \Sigma=C_i^{B}+(T^*)C_i^{A}(T^*)^T Σ=CiB+(T∗)CiA(T∗)T,尽管 T T T是三维刚体变换矩阵，其行列式为1。但无法说明 ∣ Σ ∣ |\Sigma| ∣Σ∣是常数，所以不是太明白为什么作者把这项直接忽略了,有可能是这项相当于一个 T T T的平方项,对于最小值影响不大。 ？？？？？
以上就是Generalized ICP的核心内容。有了上式，point-to-point ICP可以看成其special case,令 C i B = I , C i A = 0 即 可 C_i^B=I,C_i^A=0即可 CiB=I,CiA=0即可。point-to-plane icp也可以被纳入此框架下(这里涉及正交投影矩阵的知识正交投影阵)。
Generalized ICP的应用：plane-to-plane ICP
这一节要借助上文的结论，推导出plane-to-plane ICP
前提假设：
- 我们平常处理的点云有什么特点？它实际上是3维空间中surface的采样点集合。这意味着我们处理的是3维空间中的2维流形。现实世界中的surface，都是局部可微的，因此我们可以假设点云的局部是一个平面。
- 两个视角获取的点云，相当于从两个视角对2维流形进行了采样，在两个视角采样到同一个点的概率很小，也就是说correspondance will not be exact. 因此，每一个采样点实际上只提供了一个约束，那就是法向量的方向。
基于以上两点，我们可以假设点
P
P
P处的协方差矩阵为以下形式：在沿着基向量
e
1
e_1
e1的方向(local plane的法向量方向)方差较小，而在另外两个正交方向的方差较大。
(
ϵ
0
0
0
1
0
0
0
1
)
\left(\begin{array}{lll} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right)
⎝⎛ϵ00010001⎠⎞
设点
b
i
,
a
i
b_i,a_i
bi,ai处的法向量分别为
μ
i
和
ν
i
\mu_{i} \text { 和 } \nu_{i}
μi 和 νi，则协方差矩阵为：
C
i
B
=
R
μ
i
⋅
(
ϵ
0
0
0
1
0
0
0
1
)
⋅
R
μ
i
T
C
i
A
=
R
ν
i
⋅
(
ϵ
0
0
0
1
0
0
0
1
)
⋅
R
ν
i
T
\begin{aligned} C_{i}^{B} &=\mathbf{R}_{\mu_{i}} \cdot\left(\begin{array}{ccc} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right) \cdot \mathbf{R}_{\mu_{i}}^{T} \\ C_{i}^{A} &=\mathbf{R}_{\nu_{i}} \cdot\left(\begin{array}{ccc} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right) \cdot \mathbf{R}_{\nu_{i}}^{T} \end{aligned}
CiBCiA=Rμi⋅⎝⎛ϵ00010001⎠⎞⋅RμiT=Rνi⋅⎝⎛ϵ00010001⎠⎞⋅RνiT
其中 R x R_x Rx为旋转矩阵，可以将 e 1 e_1 e1转换到与 x x x 一致的方向。
个人理解，协方差矩阵实际上起到了为cost function T T T的每一项的不同维度加权的作用。效果是：
- 在优化过程中，沿着法向量方向的约束较紧，沿着其他两个方向的约束较松
- 如果两个法向量方向不一致的点，却因为距离较近而错误地匹配在了一起，那么以上协方差矩阵的设计，将使得沿着法向量方向的约束被削弱
图示如下：
Results
作者对三个算法进行了比较。
- standard icp: 尽管有封闭解，但为了可比较性，还是使用了共轭梯度法做优化。最大迭代次数250
- point to plane icp：最大迭代次数50
- plane to plane icp：最大迭代次数50
测试方法：
在两个已知位姿关系的点云测试算法的收敛性。
对simulated data 和real data分别进行了测试：
- simulated data 是使用SICK公司的scanner获取的，分别扫描了indoor和outdoor的场景。然后模拟了 scanner 在不同位置产生的点云，并加入Gauss噪声。
- real data 是车载激光雷达采集的。ground truth由SLAM 技术给出。
以上三个数据集：simulated data indoor, simulated data outdoor, real data ，每个都采集了多组scan pairs
初始位姿是给每组scan pair的ground truth 上加上一个均匀分布的噪声（±1.5m，±15度）产生的（每个scan pair 会测十个初始位姿）。针对不同的数据集，算法的performance以scan pairs的平移误差的均值来表征。不考虑旋转误差。
max_correspondence参数对配准结果很重要，作者测量了该参数对误差的影响：
从图中可以看出：
- 算法的平均error： standard icp > point to plane icp > generalized icp
- generalized icp 对
max correspondence参数的设置最不敏感，即使设的过大，其performance的下降也比其他两种方法要小;但是仍然可以看到，在simulated data 的表现比real data要好，这是因为real data中会有更多高频的细节信息，存在更多generalized icp处理不了的情况：两个点是错误匹配，但是法向量方向却是一致的。
扫一扫
444
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
