地面分割算法（一）：linefit_ground_segmentation

激光雷达 / lidar算法 / 地面分割

paper: Fast segmentation of 3D point clouds for ground vehicles

Github: Github-lorenwel-linefit_ground_segmentation

1 简介

这篇论文的算法是应用比较广泛的激光点云地面分割算法，在许多激光SLAM算法中都有引用。该论文方法效率高，且可以适用于具有一定坡度的地面。

算法中，将点云划分为多个扇区，一个扇区作为一个独立的处理单元。每个扇区再分成多个容器，每个容器取一个最低点，根据这些最低点拟合地面线，并以此为基础判断全部激光点是否为地面点，实现地面点云与非地面点云的分割。算法分为以下几个主要步骤：

  • 点云划分与映射

  • 地面线段拟合

  • 地面点云分割（地面点与非地面点判断）

2 点云结构化

点云结构化即：将一组无序的激光点云按照一定规则进行划分和映射，最终得到一组有序点云，也相当于是建立了索引，便于后续的算法处理。

  • 将激光点云按角度等间隔为 $N$ 个$Segment$（每个$Segment$为一个扇区），对于每个$Segment$ 按距离又等分$M$个$Bin$ （即容器），于是，每个点对应一个$(Segment,Bin)$；
  • 对于每个三维点 $P(x,y,z)$ ，平面坐标用极坐标方式表示 $P(r,\theta,z)$ ，由于每个$Segment$ 内的点被认为角度一样，于是每个$Segment$ 内角度可以忽略，用二维点 $P(r,z)$ 表示，其中 $r=\sqrt{x^2+y^2}$ ，相当于降维。
  • 计算每个 $Bin$ 内的最低点 $MinZPoint(d,z)$ 。

3 地面线段拟合

3.1 流程图

根据每个 $Bin$ 的最低点拟合地面线段 $GroundLine$ ，其论文理论部分相对简单，但代码逻辑相对复杂一些，原论文中的算法伪代码如下：

为更好地理解算法逻辑，下面为依据代码所做的流程图。提前进行如下说明：

符号 说明
binPoint 表示每个Bin中的最低点 MinZPoint(d,z)
curBinPoint 当前循环中正在处理的 binPoint
firstBinPoint 找到的第一个非空bin中的 binPoint
非空bin 有激光点落入该bin 中则非空，非空bin 才有 binPoint
linePoints 是一个vector，存储的是当前状态下，用于拟合局部地面线段的备选 binPoint
lastBinPoint 上一个入选linePoints的 binPoint，lastBinPoint=linePoints.back()
linePoints_size 上面linePoints这个vector的大小，也就是备选 binPoint 的点数量
localGroundLine 利用linePoints中的备选 binPoint拟合出的局部地面线段
groundLines 本算法的最终结果存储，是一个vector，里面元素为一个个线段，
每个线段实际保存两个点即可

基于以上符号的含义来理解以下算法流程。为了便于理解整个算法，以下的判断条件多为简写，如if curBinPoint valid，if localLine good 等，具体的判断条件后文再具体分析，此处先提纲挈领地理解整个地面线段拟合的算法逻辑。

总的来说，需要带着两个问题：

  • 满足什么条件的点可以用来拟合地面线段？
  • 什么条件下，地面线需要中断并重新开启一条线段（以符合不同坡度的地面情况）？

以上流程中，同一颜色的框图代表相同或相近的含义：

  • 蓝：开始或转入一次循环；
  • 橙：不同的判断条件；
  • 绿：拟合地面线段（注意，并不是每次拟合的都是地面线段结果，效果不佳时要剔除相关点再拟合）；
  • 黄：开始一条新的线。注意两种情况略有不同。
    • 第一种，有效点太少且又遇当前点为中断点，则删去前面的少量有效点，从中断点即当前点重新开始；
    • 第二种实则又分为两种，一是前一个线段有效保存下来，二是前段依然有效点不够，但都是lastBinPoint作为中断点，与第一种情况curBinPoint为中断点略有不同（具体原因，前一个线段有效保存可以理解，但是前段有效点不够的情况似乎与第一种并无二致，原因暂未深究）；
  • 红：保存地面线段，注意，这个步骤groundLines存储的是我们本阶段的终极目标；
  • 白：其他 less important 步骤，如push、pop等；
  • 灰：算法起止。

3.2 判断条件

  • if curBinPoint exists ?
  • if linePoints_size ≥ n ？

这两个比较简单。

判断条件 说明
if curBinPoint exists ? 有激光点落入该bin 中则非空，
非空bin 有 binPoint，即curBinPoint 存在，反之不存在。
if linePoints_size ≥ n ? linePoints中的binPoint数量大于n表示满足判断条件。
  • if curBinPoint valid ?

对应上述伪代码为第15行：

源代码为（linefit_ground_segmentation/src/segment.cc: Line 69）：

1
2
3
4
5
// Not enough points. // Add point if valid.
if (cur_point.d - current_line_points.back().d < long_threshold_ &&
    std::fabs(current_line_points.back().z - cur_ground_height) < max_start_height_) {
    current_line_points.push_back(cur_point);
}

有两个条件需要同时满足（用本文中定义的符号描述，cur_point即本文的curBinPoint，current_line_points即本文的linePoints，current_line_points.back()即本文的lastBinPoint），

① curBinPoint.d - lastBinPoint.d < long_threshold，当前点和上一个入选linePoints的binPoint的平面距离delta_d小于预设值，此处原作者选取的阈值为 1.0 米；

② lastBinPoint.height_from_ground < max_start_height，上一个备选binPoint的距地面高度小于预设值，此处原作者选取的阈值为 0.1 米；

  • if localLine good ?

对应上述伪代码为第6行：

源代码为（linefit_ground_segmentation/src/segment.cc: Line 47）：

1
2
3
4
5
6
7
if (error > max_error_ ||
    std::fabs(cur_line.first) > max_slope_ ||
    (current_line_points.size() > 2 && std::fabs(cur_line.first) < min_slope_) ||
    is_long_line && std::fabs(expected_z - cur_point.z) > max_long_height_) {
    current_line_points.pop_back();
    ...
}

源码中:

  • error：即fiterror，计算linePoints中所有点到拟合直线的距离并取其最大距离作为此拟合误差值；

  • cur_line.first：拟合直线的斜率，也即拟合的地面坡度；

  • is_long_line：curBinPoint.d - lastBinPoint.d > long_threshold，当前点和上一个备选binPoint的平面距离大于预设值，则is_long_line为true；

  • expected_z：当前点在上一个拟合地面线上的Z值

以下四个条件满足任意一个，即表示刚刚拟合的地面线不是一个“好的直线”：

  • ① 拟合误差error大于设定阈值；

  • ② 拟合的地面坡度cur_line.first大于设定阈值；

  • ③ 拟合点数量linePoints_size ≥3 且 拟合的地面坡度cur_line.first大于设定阈值；

  • ④ 同时满足curBinPoint.d-lastBinPoint.d>long_threshold，
    和abs(expected_z-cur_point.z)>max_long_height 两个阈值条件。

4 地面点云分割

简单来讲，就是计算激光点到当前及相邻的Segment中的拟合地面线段的投影误差，如果小于设置阈值，则认为是地面点，否则为非地面点。

此处关键在于找到正确的对应地面线段，要求激光点 $P(r,z)$ 的 $r$ 值在地面线段的起止点区间内 $(r_{min},r_{max})$ 方可认为是对应的地面线段，为了更有效地进行匹配，算法的匹配过程给这个起止区间加上了一个余量（或称为缓冲）kMargin，实际满足的条件为 $r\in(r_{min}-kMargin,\ \ r_{max}+kMargin)$。

具体步骤如下：

  • ① 在激光点所在的segment内，匹配地面线段，若匹配到合适的地面线段，则计算投影误差并返回；
  • ② 若在①中没有成功匹配地面线段，则在邻域segment内，重复①步骤。其中，邻域segment定义为角度相差小于阈值line_search_angle 的左右相邻segment；
  • ③ 无法匹配地面线段的激光点为非地面点；
  • ④ 已经匹配地面线段的激光点，投影误差小于阈值 max_dist_to_line 的激光点为地面点，否则为非地面点。

5 算法参数设置

前文中涉及到了不少的参数设置，主要是相关阈值，在这里总结一下。

在源代码中，参数配置文件为 linefit_ground_segmentation_ros/launch/segmentation_params.yaml

有以下参数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
n_threads: 4                # number of threads to use.

r_min: 0.5                  # minimum point distance.
r_max: 50                   # maximum point distance.
n_bins: 120                 # number of radial bins.
n_segments: 360             # number of radial segments.
     
max_dist_to_line: 0.05      # maximum vertical distance of 
                            # point to line to be considered ground.

sensor_height: 1.8          # sensor height above ground.
min_slope: 0.0              # minimum slope of a ground line.
max_slope: 0.3              # maximum slope of a ground line.
max_fit_error: 0.05         # maximum error of a point during line fit.
long_threshold: 1.0         # distance between points after which 
                            # they are considered far from each other.
max_long_height: 0.1        # maximum height change to previous point in long line.
max_start_height: 0.2       # maximum difference to estimated 
                            # ground height to start a new line.
line_search_angle: 0.1      # how far to search in angular direction 
                            # to find a line [unit: rad].

gravity_aligned_frame: ""   # Frame which has its z axis aligned with gravity. 
                            # (Sensor frame if empty.)

latch: false                # latch output topics or not
visualize: false            # visualize segmentation result - USE ONLY FOR DEBUGGING

6 效果展示

论文中的效果展示：

7 其他

博文 地面分割：Fast Segmentation of 3D Point Clouds for Ground Vehicles_TiRan_Yang的博客 中举了一个实例，对代码进行了解读，可参看。

Donate
  • © 2023 , Monicakaa .

请我喝杯咖啡吧~

支付宝
微信