改进LOAM的方法:使用CSS(曲率尺度空间)提取点云的角点

改进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} pnpn1>Dmax
∣ ∣ p n − p n − 1 ∣ ∣ ||p_n-p_{n-1}|| pnpn1是平面中两个点的欧氏距离,而 D m a x D_{max} Dmax是一个自适应变换的阈值量。
在这里插入图片描述
在这里插入图片描述

我们定义一条通过扫描点 p n − 1 p_{n-1} pn1的虚拟线,它代表能够代表可以可靠地检测到环境线的极端情况。该虚拟线与线束 r n − 1 r_{n-1} rn1构成夹角 λ \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) rn1sin(λ)=rnhsin(λΔϕ)

∣ ∣ 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)} pnhpn1=rn1sin(λΔϕ)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=pnhpn1+3σr

计算尺度空间

参数化曲线方程

  1. 计算曲线中每两两相邻点间的欧氏距离 d d d
  2. 每次计算 d d d后,对 d d d进行积分得到点 p n p_n pn位置占曲线长度的比例 t , t ∈ [ 0 , 1 ] t,t \in [0,1] t,t[0,1]
  3. 积分到曲线最后一个点,得到曲线总长度 D D D
  4. 得到曲线关于长度比例 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π 1et2/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(tu)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,σ)=tX(t,σ)=t[x(t)g(t,σ)]=x(t)(tg(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,σ)=t22X=x(t)(t22g(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}| κi1<κi>κi+1   and   κi2<κi>κi+2
意思是每连续5个连续点进行曲率值比较,选取局部极值点。

选取边缘特征点

  1. 从高尺度到低尺度寻找最终关键点
  2. 在至少13个尺度下都能找到相同的曲率极值点,则该曲率极值点入选最终关键点
  3. 最终关键点在不同尺度下被找到的次数作为点的可信度
  4. 每个簇中选取可信度最高的两个点作为sharp point
  5. 其余的最终关键点作为less sharp point
  • 7
    点赞
  • 37
    收藏
    觉得还不错? 一键收藏
  • 4
    评论
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值