LOAM算法核心解析之特征点的提取(一)

LOAM算法核心解析之特征点的提取(一)

本篇文章接好LOAM中的特征点提取算法，以及其特征点的定义方法。

LOAM算法整体的流程与传统的基于特征点的视觉SLAM很相似：
第一步、提取点云中的特征点；
第二步、使用最近邻算法计算匹配点；
第三步、使用梯度下降算法求解当前帧点云姿态。

由于LOAM是基于机械扫描式的3D激光传感器的SLAM算法，所以其特征点提取算法很大程度上也与机械扫描式激光的物理特性相关。
这里我们假设激光的坐标系为： x正–向前，y正–向右, z正–向上，这种坐标系一般是我们比较常用的车辆坐标系，其满足右手系。

这里有一个小知识点，如果我们绕某一个轴进行旋转，怎样确定哪个方向是正，哪个方向是负呢？
判断方法:假设我们要绕x轴旋转，我们用右手握住x轴，大拇指指向x轴的正向，则弯曲四指的方向为旋转的正向。

一、按激光线进行点云聚类

3D激光按线束划分种类，当前的机械扫描式的激光一般包括：16线，32线，64线等。所以LOAM算法的第一步是将接收到的3D点云，根据其与水平面的夹角划分到激光的不同线上。
LOAM对点云的预处理首先会把点按激光的线束进行分割，将相同线束上的点划分到一起。其划分方式就是依据3D点坐标，求得每个点相对与激光坐标x-y平面的夹角，依据不同夹角划分道不同的线束上，其公式为:
$$\theta =arctan(z,\sqrt{x^2+y^2})$$

其中$\theta$为夹角，x,y,z为激光坐标系下点的坐标。

二、特征点筛选条件

为了提取点云中稳定特征点，用于定位匹配，作者认为形状尖锐的角点、形状平滑的角点和平面点是点云中的特征点。所以定义了一种特征点的划分算法，具体算法为：

计算当前点前后个5个点与当前的点的曲率(实际计算方差)，其公式为：
$$c=\frac{1}{|S|\cdot ||X_{(k,i)}^L||}||\sum _{j\in S,j\ne i}(X_{(k,i)}^L-X_{(k,j)}^L)||$$

实际代码中的计算第i个点的曲率的公式:
$$c=\frac{1}{2\ast n}\sum _j^{(-n,n)}(X_{(k,i)}^L-X_{(k,i-j)}^L)$$

其中, c表示计算得到的曲率；$n$ 表示计算点云的区域，是以当前点i为中心，向前后各取n个点；k表示第k帧点，L表示是在激光坐标下。

1. 形状尖锐的角点
   对所有计算曲率进行排序，通过找到n（可以设置）个大曲率的top点，认为是形状尖锐的点

2. 形状平滑的角点
   然后提取m(通过可设置的阈值进行提取)个曲率较大的点作为形状平滑的角点

3. 平面点
   然后找到k个(通过可设置的阈值进行提取)曲率小于阈值的点认为是平面点

这样就可以在一帧中得到有效的点数了。而在论文中是对整个扫描进行化段，分四段，每段各取两个边缘点和4个平面点.

三、过滤一些异常点

通过上面的特征提取的特征点仍然会存在一些不稳定的点，为了保证算法的问题去要对其进行识别，并剔除。

异常点定义为：

1. 该点的周围的点尽量不要被再被取到，这样可以使整体的特征点分布更加的平均
2. 该点不能与雷达扫描束过于平行，这样也会出现问题

具体的表现形式如图：