Scan Registration with Multi-Scale K-Means Normal Distributions Transform
这篇论文和之后的两篇论文都来自同一作者,主要是描述了对标准NDT算法的改进以提升收敛性和降低计算量。本文主要贡献是使用K-均值聚类生成高斯分布,并在不同尺度上进行配准,这个思想和多层光流法类似。同时,为了克服仅使用单个高斯分布进行评估导致的代价函数的不连续性,使用了所有聚类生成的高斯分布进行评估,由于使用了K-均值聚类方式,生成的高斯分布的数量要远小于基于固定尺寸栅格划分的高斯分布的数量,所以这种修改带来的计算量的提升是可以接受的。但是由于K-均值聚类是一种需要人为给定聚类数量的方法,需要进行超参调节。这个缺点也是作者后续两篇文章的主要改进点。
摘要
已经证明,正态分布变换(NDT)扫描配准算法可以产生良好的结果,但是,如果初始参数误差较大,则有收敛到局部极小值的趋势。为了提高NDT的收敛速度,提出了一种多尺度K-均值NDT变体。这种方法使用K-均值聚类来划分点云,并执行多个聚类大小的优化步骤。K-均值聚类方法保证了优化会收敛,因为它解决了标准NDT算法中代价函数不连续的问题。新算法的优化步骤表现在尺度的减小上,这极大地改善了收敛域。实验表明,这种方法可以用来配准具有较大的初始误差且只有部分区域重叠的扫描。
引言
扫描配准算法在移动机器人的建图技术中扮演了重要的角色。许多传感器,如RGBD相机、雷达和双目相机,均以点云的形式提供机器人周围环境的信息。在点云数据有几何重叠的应用程序中,扫描配准技术可用于确定他们之间的相对变换。这有助于点云数据的集成,在未知环境中进行自动操作通常需要点云数据,并且拥有较广泛的应用,如自动挖掘、监视、搜索、救援和探索。
许多SLAM算法依赖例如迭代最近点(ICP)之类的算法来估计两个重叠点云之间的相对变换。Besland McKay[1]、Chenand Medioni[2]和Zhang[3]独立介绍了ICP算法。ICP算法试图找到使相应点和最近邻点之间的欧几里德距离最小化的变换参数。
标准ICP算法没有考虑点云的潜在表面结构。为了解决这个问题,Segal等人引入了广义ICP的思想[4]。这种方法使用局部邻域点计算表面结构,并在优化过程中使用这些额外的信息来生成更高质量的扫描匹配。ICP方法的一个缺点是最近邻对应关系的生成。这一步通常是计算昂贵的并且已经被证明是ICP算法的瓶颈。
正态分布变换(NDT)是一种相对较新的方法,最初由biber和strasser为扫描配准和建图[6]提出。后来被Magnusson等人用于3D矿山测绘任务。这种方法是可取的,因为它不需要计算显式点的对应关系。NDT算法将底层扫描表示为一组高斯分布,这些高斯分布将表面局部建模为概率密度函数。它是一个基于优化的方法,通过在概率密度函数中评估由参数估计 p p p变换后的点云中的点来获得一个最优的参数估计 p p p。为了配准两个点云,NDT试图寻找变换参数来最大化这个分数。
非线性优化中标准的NDT公式易受局部最小值的影响。图1给出了NDT3D代价函数在
x
x
x和
θ
\theta
θ平面上的投影。
本文提出了一种提高NDT收敛性的方法。扫描基于栅格的划分被多尺度K-均值聚类技术取代。这种聚集在扫描中捕捉了从粗到细的细节。扫描的K-均值聚集消除了由标准NDT算法中的栅格单元边界引起的代价函数的不连续性。这保证了非线性优化是良好定义的,并且所有标准的非线性优化算法都将收敛到满足最佳性必要条件的解[9]。为了增加收敛域,优化步在不同的尺度上执行,因为当前尺度的优化是用前一尺度的解初始化的。这种从粗到精的方法避免了算法收敛到局部极小值,并被实验证明在大的初始变换误差的情况下可以可靠的收敛到全局极小值。点云的K-均值聚集先前被马格努松尝试过,但是是在单一的尺度上,并且只在最近的高斯聚类上评估扫描点[10]。聚类大小的选择也是一个问题,因为聚类尺寸太小或太大会导致配准结果不佳。
问题描述
A. 2D配准
扫描配准算法试图在两个点云扫描(参考扫描和场景扫描)之间寻找最优变换。目标是确定最佳对齐两者的变换参数,满足两个扫描的重叠程度尽可能的高。定义参考扫描中点的集合
Y
=
{
y
1
,
.
.
.
,
y
N
y
}
Y = \{y_1,...,y_{N_y}\}
Y={y1,...,yNy}其中
y
i
∈
R
2
,
i
∈
{
1
,
.
.
.
,
N
Y
}
y_i \in \mathbb{R}^2, i \in \{1,...,N_Y\}
yi∈R2,i∈{1,...,NY}和场景扫描中的点
X
=
{
x
1
,
.
.
.
,
x
N
x
}
X=\{x_1,...,x_{N_x}\}
X={x1,...,xNx}其中
x
j
∈
R
2
,
j
∈
{
1
,
.
.
.
,
N
X
}
x_j \in \mathbb{R}^2,j \in \{1,...,N_X\}
xj∈R2,j∈{1,...,NX}。使用参数估计
p
=
[
t
x
,
t
y
,
θ
]
T
∈
R
2
×
[
0
,
2
π
]
p = [t_x,t_y,\theta]^T \in \mathbb{R}^2 \times [0,2 \pi]
p=[tx,ty,θ]T∈R2×[0,2π]可以给出从一个点
x
x
x场景到参考坐标系的变换
T
Y
(
p
,
x
)
=
[
c
o
s
θ
−
s
i
n
θ
s
i
n
θ
c
o
s
θ
]
x
+
[
t
x
t
y
]
(1)
T_Y(p,x) = \begin{bmatrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{bmatrix}x + \begin{bmatrix} t_x \\ t_y \end{bmatrix} \tag{1}
TY(p,x)=[cosθsinθ−sinθcosθ]x+[txty](1)
其中
θ
\theta
θ是两个坐标系之间的旋转,而
[
t
x
,
t
y
]
[t_x,t_y]
[tx,ty]是平移。
B. 正态分布变换
正态分布变换(NDT)是一种方法,通过这种方法,点云的各部分被映射并表示为栅格结构中的高斯分布。然后,通过将参考扫描中点所占据的空间细分为一组栅格单元
c
c
c(2D的长方体单元和3D的立方体单元)。定义参考扫描中在栅格
c
i
c_i
ci中的点的集合为
Y
c
i
=
{
y
1
c
I
,
.
.
.
,
y
N
c
i
c
i
}
Y^{c_i} = \{y_1^{c_I},...,y_{N_{c_i}}^{c_i}\}
Yci={y1cI,...,yNcici},满足$y_k^{c_i} \in c_i $。对于每个栅格
c
i
c_i
ci,参考扫描中包含在该栅格中的点用于生成一个具有代表性的高斯分布
N
(
μ
c
i
,
Σ
c
i
)
\mathcal{N}(\mu_{c_i},\Sigma_{c_i})
N(μci,Σci)的均值
μ
c
i
\mu_{c_i}
μci和协方差矩阵
Σ
c
i
\Sigma_{c_i}
Σci。这个概率分布函数可以被解释为一个生成过程,它模拟了单元格中点
Y
c
i
Y^{c_i}
Yci的局部表面。假设参考扫描中表面点的位置是从这个分布中刻画出来的,在栅格
c
i
c_i
ci内具有测量点
y
y
y的可能性可以建模为
ρ
(
y
)
=
exp
(
−
(
y
−
μ
c
i
)
T
Σ
c
−
1
(
y
−
μ
c
i
)
2
)
\rho(y) = \exp(- \frac{(y-\mu_{c_i})^T \Sigma_{c}^{-1}(y - \mu_{c_i})}{2})
ρ(y)=exp(−2(y−μci)TΣc−1(y−μci)),其中
μ
c
i
\mu_{c_i}
μci和
Σ
c
i
\Sigma_{c_i}
Σci是栅格
c
i
c_i
ci的采样均值和协方差矩阵。2D激光扫描的NDT概率分布图如图2所示。
由于电云现在是由一组分段连续的和分段可微的高斯分布的和来建模的,所以可以使用数值优化工具来将配准参考扫描和场景扫描。可以计算出一个适应度代价,
s
:
R
2
×
[
0
,
2
π
]
s:\mathbb{R}^2 \times [0,2\pi]
s:R2×[0,2π],它量化了扫描
X
X
X和扫描
Y
Y
Y之间的重合度,并可以表示为
s
(
p
)
=
−
∑
k
=
1
N
x
ρ
Y
(
T
(
p
,
x
k
)
)
s(p) = - \sum_{k=1}^{N_x}{\rho_Y(T(p,x_k))}
s(p)=−∑k=1NxρY(T(p,xk))。它定义了扫描
X
X
X中每个点可以从被参数猜测值
p
p
p变换后的扫描
Y
Y
Y的NDT表示中刻画出来的概率和。使用这个代价函数,NDT扫描配准算法的目标是找到一个参数估计值
p
p
p以最小化代价函数
s
s
s。应该注意的是,每个点都对代价函数的梯度
g
g
g和海塞矩阵
H
H
H有贡献。
在执行优化时,参数估计中的任意微小变化都可能导致场景扫描中的点跨越栅格边界,从而导致代价函数不连续。这意味着代价函数是不平滑的,梯度和海塞矩阵不存在于这些栅格边界的变换空间中。Magnussonet等人使用三线性插值方案来解决这个问题,该方案在计算单个场景扫描中点对梯度和海塞矩阵的贡献时考虑了相邻栅格的影响[8]。然而,这并不能解决这个问题,因为对于任何变换参数的小的改变,边界穿越仍将导致代价函数的有限的改变。
在标准NDT算法中,优化使用参数估计
p
0
p_0
p0初始化,并被执行直到满足某些收敛阈值,比如
∣
g
∣
<
δ
|g| < \delta
∣g∣<δ,其中
δ
\delta
δ是用户定义的代价函数处于极小值时候的梯度期望范数。因为无法保证,所以收敛迭代次数由
i
t
e
r
m
a
x
iter_{max}
itermax限制。定义函数
W
g
(
x
k
,
p
)
W_g(x_k,p)
Wg(xk,p)和W_{H}(x_k,p),它们使用场景扫描中的一个点
x
k
x_k
xk来计算对应的梯度和海塞矩阵贡献。Biber使用带线性搜索的牛顿方法来执行优化,优化的全部细节和
W
g
(
x
k
,
p
)
W_g(x_k,p)
Wg(xk,p)和
W
H
(
x
k
,
p
)
W_H(x_k,p)
WH(xk,p)的详细描述可以在[6]中找到。NDT方法在算法1中进行了总结。
C. K-均值聚类
K-均值聚类是一种数据划分技术,给定一组 n n n个数据点,KaTeX parse error: Expected 'EOF', got '}' at position 20: …d_1,d_2,...,d_n}̲,试图将数据点 D D D划分到 k k k个集合 S = { s 1 , s 2 , . . . , s k } S=\{s_1,s_2,...,s_k\} S={s1,s2,...,sk}中。关联度量是数据集中的点和它们的对应的最近的聚类均值之间的平方距离。K-均值试图将数据点分到 k k k个集合中,使得函数 f k m = ∑ i = 1 k ∑ d j ∈ s i ∣ ∣ x j − μ i ∣ ∣ 2 f_{km} = \sum_{i=1}^{k} \sum_{d_j \in s_i}{|| x_j - \mu_i ||^2} fkm=∑i=1k∑dj∈si∣∣xj−μi∣∣2最小化。术语 μ i \mu_i μi是分配到集合 s i s_i si中点的均值。K-均值算法通常使用迭代的方法进行执行,有两个主要步骤。给定初始 k k k个初始聚类和聚类均值,第一步将 D D D中数据点分配给均值距离当前点最近的聚类。第二步,使用分配给每个聚类中的点来重新计算聚类均值。重复这些步骤,直到在 t t t次和 t − 1 t-1 t−1次迭代之间均值的移动 α \alpha α低于某个阈值 δ \delta δ。用于聚类的K-均值算法在算法2中总结。
本文提出的方法:多尺度K-均值聚类NDT(MSKM-NDT)
为了克服NDT算法收敛性差的缺点,提出了一种多尺度配准方法。该方法使用K-均值聚类对不同尺度的扫描进行聚类,并在不断减小尺度的情况下进行优化。多尺度优化通过由粗到细的优化提升了算法的收敛性并避免了算法陷入局部极小值。为了适应这些,标准的NDT方法在以下方面进行了修改:
1)
NDT 中用于分割扫描的基于栅格的方法被K-均值算法取代,用于将扫描聚类为 k k k个聚类。令所有聚类的集合为 Γ = { γ 1 , γ 2 , . . . , γ k } \Gamma = \{\gamma_1,\gamma_2,...,\gamma_k\} Γ={γ1,γ2,...,γk},其中每个聚类有 N γ j N_{\gamma_j} Nγj个点,并且 ∑ i = 1 k N γ j = N y \sum_{i=1}^k{N_{\gamma_j}} = N_y ∑i=1kNγj=Ny。其中,标准 NDT将每个栅格内的点建模为高斯分布,MSKM-NDT建模每个K-均值聚类中的点为高斯分布 ρ γ j \rho_{\gamma_j} ργj
2)
标准的NDT算法对一个固定的栅格间距执行优化步骤,而MSKM-NDT在逐渐减小的尺度上执行优化步骤,随着尺度的减小增加聚类的数量。尺度集合 Φ = { ϕ 1 , ϕ 2 , . . . , ϕ N } \Phi = \{\phi_1,\phi_2,...,\phi_N\} Φ={ϕ1,ϕ2,...,ϕN}是按降序选择的。这导致了一个从到粗到细的优化方案,旨在收敛到代价函数的全局最小值。
3)
为了适应这种类型的参考扫描聚类,标准的NDT代价计算被修改。标准 NDT评估被变换点 x d x_d xd落入的栅格内的高斯分布。对于 MSKM-NDT,每个被变换点 x d x_d xd是相对于当前尺度 ϕ i \phi_i ϕi 中的所有聚类分布进行评估的,其中最强的代价贡献来自最有可能的聚类。这导致在多尺度优化的每个阶段产生连续且可微的代价函数。
使用K-均值聚类两种不同尺度上的参考扫描聚类示例如图 3 所示。
椭圆代表聚类点的高斯分布的标准偏差。这些数字表明,在较大尺度上聚类捕获扫描范围内的非常粗糙的特征,在较小尺度上聚类捕获更精细的细节。在较大尺度上,代价函数具有更宽的收敛范围,但是由于聚类的粗糙程度,在对应尺度上的极小值可能不对应真实的变换参数。在较小的尺度上,代价函数的收敛范围更窄,但是全局最小值可能对应于真实的变换参数。这个想法如图 4 所示。
在这种多尺度方法中,一个尺度
ϕ
i
\phi_i
ϕi的优化是用前一尺度
ϕ
i
−
1
\phi_{i-1}
ϕi−1的参数解初始化的。 MSKM-NDT算法总结在算法3中。
实验结果
这篇文章由于已经存在后续的改进版本,实验结果的意义不大,就不描述了。
结论
在这项工作中,提出了一种改进的NDT算法,称为多尺度K-均值NDT(MSKM-NDT)。与标准NDT算法相比,所提出的方法使用K-均值聚类对扫描在不同尺度上进行划分。然后在多个尺度上执行优化步骤。扫描的K-均值聚类消除了标准NDT算法中由栅格边界引起的代价函数的不连续性。结果是平滑且可微的代价函数,保证了优化在每个尺度上的收敛性。由粗到细的方法已被实验证明,与标准NDT相比,它可以避免陷入局部极小值并且提高算法的收敛性。该领域的未来工作包括将结果扩展到3D,优化算法的实现以实现实时性能,与其他最先进的方法进行比较,以及在更大类别的环境和标准数据集上评估算法。