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 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个连续点进行曲率值比较,选取局部极值点。