改进LOAM的方法:使用CSS(曲率尺度空间)提取点云的角点
参考资料
[1] Gonzalez C , Adams M . An improved feature extractor for the Lidar Odometry and Mapping (LOAM) algorithm[C]// 2019 International Conference on Control, Automation and Information Sciences (ICCAIS). 2019.
开源代码地址:
css_loam_velodyne
问题模型
LOAM中使用简单的曲率求取公式提取出角点和平面点,虽然提取特征点时速度较快,但是提取到的角点中含有很大一部分属于噪声类的点(如:树叶、草地等),从LOAM的基于线特征和基于面特征的ICP方法来看,这些点是降低匹配精度的因素,而且这些点很大程度上增加了计算负担,因此需要一种方法能够提取到空间角度的同时,过滤掉这些无益于匹配的噪声点。
基于曲率尺度空间(Curvature Scale Space – CSS)的特征提取算法可以解决该问题。
自适应断点检测
最简化的断点检测公式:
∣
∣
p
n
−
p
n
−
1
∣
∣
>
D
m
a
x
||p_n-p_{n-1}||>D_{max}
∣∣pn−pn−1∣∣>Dmax
∣
∣
p
n
−
p
n
−
1
∣
∣
||p_n-p_{n-1}||
∣∣pn−pn−1∣∣是平面中两个点的欧氏距离,而
D
m
a
x
D_{max}
Dmax是一个自适应变换的阈值量。
我们定义一条通过扫描点
p
n
−
1
p_{n-1}
pn−1的虚拟线,它代表能够代表可以可靠地检测到环境线的极端情况。该虚拟线与线束
r
n
−
1
r_{n-1}
rn−1构成夹角
λ
\lambda
λ,它决定了最坏情况下的点
p
n
p_n
pn的位置,在该约束下,有约束公式:
r
n
−
1
⋅
s
i
n
(
λ
)
=
r
n
h
⋅
s
i
n
(
λ
−
Δ
ϕ
)
r_{n-1}\cdot sin(\lambda)=r_n^h \cdot sin(\lambda - \Delta \phi)
rn−1⋅sin(λ)=rnh⋅sin(λ−Δϕ)
∣ ∣ p n h − p n − 1 ∣ ∣ = r n − 1 ⋅ s i n Δ ϕ s i n ( λ − Δ ϕ ) ||p_n^h-p_{n-1}||=r_{n-1} \cdot \frac{sin{\Delta \phi}}{sin(\lambda - \Delta \phi)} ∣∣pnh−pn−1∣∣=rn−1⋅sin(λ−Δϕ)sinΔϕ
D m a x = ∣ ∣ p n h − p n − 1 ∣ ∣ + 3 σ r D_{max}=||p_n^h-p_{n-1}||+3\sigma_r Dmax=∣∣pnh−pn−1∣∣+3σr
计算尺度空间
参数化曲线方程
- 计算曲线中每两两相邻点间的欧氏距离 d d d
- 每次计算 d d d后,对 d d d进行积分得到点 p n p_n pn位置占曲线长度的比例 t , t ∈ [ 0 , 1 ] t,t \in [0,1] t,t∈[0,1]
- 积分到曲线最后一个点,得到曲线总长度 D D D
- 得到曲线关于长度比例 s s s的参数方程: C = { x ( t ) , y ( t ) } C=\{x(t),y(t)\} C={x(t),y(t)}
曲率计算
曲率:
κ
=
d
ψ
d
s
=
1
ρ
\kappa = \frac{d\psi}{ds}=\frac{1}{\rho}
κ=dsdψ=ρ1
其中的
ρ
\rho
ρ就是曲线在该点位置对应的曲率半径。
曲率
κ
\kappa
κ可以由
x
(
t
)
x(t)
x(t)和
y
(
t
)
y(t)
y(t)的导数计算:
y
′
=
d
y
d
x
y
′
′
=
d
2
y
d
x
2
y'=\frac{dy}{dx}\ \ \ y''=\frac{d^2y}{dx^2}
y′=dxdy y′′=dx2d2y
再得到曲率:
κ
=
y
′
′
(
1
+
(
y
′
)
2
)
3
/
2
\kappa = \frac{y''}{(1+(y')^2)^{3/2}}
κ=(1+(y′)2)3/2y′′
将导数自变量转为
t
t
t
x
˙
=
d
x
d
t
x
¨
=
d
2
x
d
t
2
y
˙
=
d
y
d
t
y
¨
=
d
2
y
d
t
2
\dot{x}=\frac{dx}{dt}\ \ \ \ddot{x}=\frac{d^2x}{dt^2} \\ \dot{y}=\frac{dy}{dt}\ \ \ \ddot{y}=\frac{d^2y}{dt^2}
x˙=dtdx x¨=dt2d2xy˙=dtdy y¨=dt2d2y
然后有:
y
′
=
y
˙
x
˙
y
′
′
=
x
˙
y
¨
−
y
˙
x
¨
x
˙
3
y'=\frac{\dot y}{\dot x} \\ y''=\frac{\dot{x}\ddot{y}-\dot{y}\ddot{x}}{\dot{x}^3}
y′=x˙y˙y′′=x˙3x˙y¨−y˙x¨
为了计算不同尺度下的曲线,需要将曲线方程与宽度为
σ
\sigma
σ的一维高斯内核
g
(
t
,
σ
)
g(t,\sigma)
g(t,σ)进行卷积:
g
(
t
,
σ
)
=
1
σ
2
π
e
−
t
2
/
2
σ
2
g(t,\sigma)=\frac{1}{\sigma \sqrt{2 \pi}}e^{-t^2/2\sigma^2}
g(t,σ)=σ2π1e−t2/2σ2
将
x
(
t
)
x(t)
x(t)与高斯内核卷积定义为
X
(
t
,
σ
)
X(t,\sigma)
X(t,σ)
X
(
t
,
σ
)
=
x
(
t
)
⊛
g
(
t
,
σ
)
=
∫
−
∞
∞
x
(
u
)
1
σ
2
π
e
−
(
t
−
u
)
2
/
2
σ
2
d
u
X(t,\sigma)=x(t)\circledast g(t,\sigma)\\ =\int_{-\infty}^{\infty}x(u)\frac{1}{\sigma\sqrt{2\pi}}e^{-(t-u)^2/2\sigma^2}du
X(t,σ)=x(t)⊛g(t,σ)=∫−∞∞x(u)σ2π1e−(t−u)2/2σ2du
Y
(
t
,
σ
)
Y(t,\sigma)
Y(t,σ)定义类似。
X
(
t
,
σ
)
X(t,\sigma)
X(t,σ)关于
t
t
t的导数计算:
X
˙
(
t
,
σ
)
=
∂
X
(
t
,
σ
)
∂
t
=
∂
[
x
(
t
)
⊛
g
(
t
,
σ
)
]
∂
t
=
x
(
t
)
⊛
(
∂
g
(
t
,
σ
)
∂
t
)
\dot X(t,\sigma)=\frac{\partial X(t,\sigma)}{\partial t}= \frac{\partial [x(t)\circledast g(t,\sigma)]}{\partial t} \\ =x(t) \circledast (\frac{\partial g(t,\sigma)}{\partial t})
X˙(t,σ)=∂t∂X(t,σ)=∂t∂[x(t)⊛g(t,σ)]=x(t)⊛(∂t∂g(t,σ))
如此类推:
X
¨
(
t
,
σ
)
=
∂
2
X
∂
t
2
=
x
(
t
)
⊛
(
∂
2
g
(
t
,
σ
)
∂
t
2
)
\ddot X(t,\sigma)=\frac{\partial^2X}{\partial t^2}=x(t) \circledast (\frac{\partial^2 g(t,\sigma)}{\partial t^2})
X¨(t,σ)=∂t2∂2X=x(t)⊛(∂t2∂2g(t,σ))
Y
˙
(
t
,
σ
)
\dot Y(t,\sigma)
Y˙(t,σ)和
Y
¨
(
t
,
σ
)
\ddot Y(t,\sigma)
Y¨(t,σ)的定义亦然。
那么,各尺度下的曲率计算公式:
κ
(
t
,
σ
)
=
X
˙
(
t
,
σ
)
Y
¨
(
t
,
σ
)
−
Y
˙
(
t
,
σ
)
X
¨
(
t
,
σ
)
(
X
˙
(
t
,
σ
)
+
Y
˙
(
t
,
σ
)
)
3
/
2
\kappa(t,\sigma)=\frac{\dot X(t,\sigma)\ddot Y(t,\sigma) - \dot Y(t,\sigma)\ddot X(t,\sigma)}{(\dot X(t,\sigma)+\dot Y(t,\sigma))^{3/2}}
κ(t,σ)=(X˙(t,σ)+Y˙(t,σ))3/2X˙(t,σ)Y¨(t,σ)−Y˙(t,σ)X¨(t,σ)
寻找曲率极值
在不同尺度下寻找曲率极值点,其入选的条件:
∣
κ
i
−
1
∣
<
∣
κ
i
∣
>
∣
κ
i
+
1
∣
a
n
d
∣
κ
i
−
2
∣
<
∣
κ
i
∣
>
∣
κ
i
+
2
∣
|\kappa_{i-1}|<|\kappa_i|>|\kappa_{i+1}|\ \ \ and\ \ \ |\kappa_{i-2}|<|\kappa_i|>|\kappa_{i+2}|
∣κi−1∣<∣κi∣>∣κi+1∣ and ∣κi−2∣<∣κi∣>∣κi+2∣
意思是每连续5个连续点进行曲率值比较,选取局部极值点。
选取边缘特征点
- 从高尺度到低尺度寻找最终关键点
- 在至少13个尺度下都能找到相同的曲率极值点,则该曲率极值点入选最终关键点
- 最终关键点在不同尺度下被找到的次数作为点的可信度
- 每个簇中选取可信度最高的两个点作为sharp point
- 其余的最终关键点作为less sharp point