地面分割算法（二）：Ground_removal in LeGO-LOAM

2023-06-30

paper: LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

Github: LeGO-LOAM - (RobustFieldAutonomyLab)

1 前言

在LeGO-LOAM中，Ground removal 部分引用了上文地面分割算法（一）：linefit_ground_segmentation 中介绍的论文 Fast segmentation of 3D point clouds for ground vehicles，但只是用了其中考虑地面坡度的思想，具体的代码实现有非常大的区别，包括点云的结构化方式和具体的地面滤除实施，并且代码非常的简化精炼。

Ground removal 部分的核心代码位于 LeGO-LOAM\LeGO-LOAM\src\imageProjection.cpp 文件中。主要有两部分：点云结构化 projectPointCloud() 和地面滤除 groundRemoval()。

2 点云结构化

主要原理

根据每个点的水平角度和垂直角度，再预先设置两个方向的角度分辨率，可计算得到每个点在这两个方向上的索引，将点云重新映射为基于图像的组织方式。

核心代码所在：LeGO-LOAM\LeGO-LOAM\src\imageProjection.cpp\projectPointCloud()

角度分辨率的设置

可以参考激光雷达设备手册上的参数，一般都有关于激光点云在水平和垂直两个方向上的角度分辨率，可直接将其作为我们需要的阈值，这样可以最大程度保证每个点都能有效利用。事实上，也可以通过这两个阈值的设置（给定一个更大的值，如2倍于设备本身的角度分辨率），在点云映射的同时达到降采样的目的，这个可以根据实际需求。

源码：

1
2
3

/// location: LeGO-LOAM\LeGO-LOAM\include\utility.h
extern const float ang_res_x = 0.2; // degree
extern const float ang_res_y = 2.0;

计算水平角和垂直角

直接基于点坐标计算即可。垂直角$\theta_{v}$ ，水平角$\theta_{h}$ 。
$$
{\theta_{v}}i=\arctan{(\frac{z_i}{\sqrt{x_i^2+y_i^2}})} \\
{\theta{h}}_i=\arctan{(\frac{x_i}{y_i})}
$$
注：以上水平角公式是LeGO-LOAM中的计算方法，这个与坐标系定义和水平角起始方向选取有关。这里是以激光雷达坐标系的y轴正方向为0。这里用 $\arctan{(\frac{x_i}{y_i})}$ 还是 $\arctan{(\frac{y_i}{x_i})}$ 来计算依据实际需求而定。

源码：

/// location: LeGO-LOAM\LeGO-LOAM\src\imageProjection.cpp\projectPointCloud()
float distance = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y);
verticalAngle = atan2(thisPoint.z, distance) * 180 / M_PI;
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

计算激光点在图像上的坐标

这个坐标也就是上述【主要原理】中所讲的索引值。

1
2
3

/// location: LeGO-LOAM\LeGO-LOAM\src\imageProjection.cpp\projectPointCloud()
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;

这里的 ang_bottom 的引入和-90.0、+ Horizon_SCAN/2 都是为了将索引值转化为大于等于0的正整数，以符合图像中坐标的定义，也正是其所谓索引。

特别地，如果是机械雷达，rowIdn 其实就是 ring，可以免去这部分的计算，直接将 ring 值赋给 rowIdn 即可。

举例说明

以机械雷达为例：

图像行：每一线激光点就是图像中的一行点，并且图像中的任意一行的首尾其实也是相邻的，这点在后续的点云聚类中有体现（不在此文的地面点云分割范围）；

图像列：下图中两圈表示机械雷达的两线点云，红框中是同一个方向的分属于两线的两个点，映射到图像中就是同一列中相邻的两个像素点。

3 地面滤除

LeGO-LOAM中地面滤除算法非常简单。

从靠近激光雷达的点开始判断。对于任意当前一点，再取出下一点（下一行中同一列的激光点），计算这两个点形成的坡度大小，再依据给定的坡度阈值（文中取值为10度），判断其是否为地面点。

激光点是有可能有无效点的，没有收到回波的点一般存为Nan点，那么如果当前或者下一点是无效点，这个坡度值就没办法计算了，那么当前点就标记为 -1；

对于正常的两个点，如果计算坡度小于阈值，则两点同为地面点，标记为 1；

如果计算坡度大于阈值，则两点均为非地面点，标记为 0。

结束了，就这么简单。

可能会有人要问了，那如果是一个障碍物目标，它的上表面比较平整，不是就被误判为地面点了吗？

是这样，肯定是有这个的概率的。但是本文的地面分割是为了更好地做后续的特征匹配的工作，这里不是为了做障碍物识别，所以这一点误判影响并不大；并且，这些误判点不一定会被识别为特征点，可以说又降低了其影响；再者，论文中的实验大多是周围是植被的环境，且安装位置也比较低，不太容易出现上面所说的这种情况。

所以要注意，如果是用于自动驾驶的避障中，可能要多关注这个问题。特别是安装在巴士顶部的激光雷达，能够看到前车的顶部，特别是挂车后的拖挂平板这种，可能就漏掉了。这是需要注意的。

4 点云的代码结构

这部分的代码组织个人感觉还是很好的，可以借鉴参考。

首先根据两个方向的视场角和角度分辨率，可以得到图像的大小。这个size一般事先计算当做参数输入。

1
2
3

/// location: LeGO-LOAM\LeGO-LOAM\include\utility.h
extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 1800;

这里用了三个变量来存储结构化的点云，rangeMat，groundMat，fullCloud

cv::Mat rangeMat; // range matrix for range image
cv::Mat groundMat; // ground matrix for ground cloud marking
rangeMat  = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0));

// projected velodyne raw cloud, but saved in the form of 1-D matrix
pcl::PointCloud<PointType>::Ptr fullCloud; 
fullCloud->points.resize(N_SCAN*Horizon_SCAN);
std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint);

rangeMat、groundMat 为图像形式，分别存储距离值和是否地面点的标签。fullCloud 为 pcl 的点云格式，并且与图像格式一一对应，fullCloud 中 index 为：

/// rowIdn       : point index of image row
/// columnIdn    : point index of image column
/// Horizon_SCAN : size of each image row
index = columnIdn  + rowIdn * Horizon_SCAN;

如此对应，便于点云组织以及后续算法调用。

以下是论文中特征提取的过程。

Donate