地面分割：Fast Segmentation of 3D Point Clouds for Ground Vehicles

论文链接：Fast segmentation of 3D point clouds for ground vehicles | IEEE Conference Publication | IEEE Xplore

代码链接：GitHub - lorenwel/linefit_ground_segmentation: Ground Segmentation

论文的核心思想：

相比Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications，这篇论文的逻辑比较复杂，但思想比较好。

• 将雷达点云等间隔为N个Segment，每个Segment的角度为Δα，每个Segment又等分M个Bin，即segments_(segment )(bin)。
  

• 将当前帧所有点云投影到segments_中，也就是降维：将p(x,y,z)三维，降到二维p(d, z)。
  

• 在投影的过程中获取每个点对应的(d,z)，及每个bin中最小的(d,z)

• 对每个segment拟合直线z = kd + b，这里的k指的的路面的坡度。

• 基于拟合的直线判断点到路面的高度，如果高度小于阈值，则该点是路面点，否则为障碍物点。

• 如果路面有坡度，则拟合的直线是分段的，不同系数的(k,b)代表不同坡度的路面。我们将通过代码加深了解论文的核心思想。

这种方法可以适用于具有坡度的路面，作者在论文中也给出效果。

视频动态检测效果：

接下来是代码的走读：

初始化

​ 将雷达点云分割多少个Segment及每个Segment含有多少Bin。

GroundSegmentation segmenter(params);
//每个点云对应的类别，地面点和非地面点
std::vector<int> labels;

segmenter.segment(cloud, &labels);

参数初始化

​ 以下是作者代码中的原始参数，可根据项目中的实际情况设置合适的参数。从参数中可看出，共有360个Segment，每个Segment由120个bin：

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.
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 [rad].

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

其他信息初始化

​ 这里主要是点云标签、每个点云对应的(d,z)、每个点云所在的(segment, bin)初始化，且初始化的大小为点云的个数。

//  每一个点云的标签
  segmentation->resize(cloud.size(), 0);
//  每个点云对应的（segment，bin）
  bin_index_.resize(cloud.size());
//  每一个点对应的d、z
  segment_coordinates_.resize(cloud.size());

点云投影

核心思想

​ 将点云从三维p(x,y,z)将到二维p(d, z)，虽然点云的维度减少，但点云的有效信息是不减少的：

• d = sqrt(x*x + y*y) ，相当于把笛卡尔坐标系转换到极坐标系。
• 保留Z轴的信息，要基于z的高度信息判断是否该点是否为路面点。

点云降维

以下是点云的投影的详细过程：

insertPoints(cloud);

点云投影过程中，采用多线程的并行运算，将所有点云分割为params_.n_threads份，每份一个线程进行处理。

void GroundSegmentation::insertPoints(const PointCloud& cloud) {
  std::vector<std::thread> threads(params_.n_threads);
  const size_t points_per_thread = cloud.size() / params_.n_threads;
  // Launch threads.
  /*根据我们设定的数目来将整个的点云分为几个部分开始处理，利用多线程来处理*/
  for (unsigned int i = 0; i < params_.n_threads - 1; ++i) {
    const size_t start_index = i * points_per_thread;
    const size_t end_index = (i+1) * points_per_thread - 1;
    threads[i] = std::thread(&GroundSegmentation::insertionThread, this,
                             cloud, start_index, end_index);
  }
  // Launch last thread which might have more points than others.
  /*启动最后一个可能含有更多点云的线程*/
  const size_t start_index = (params_.n_threads - 1) * points_per_thread;
  const size_t end_index = cloud.size() - 1;
  threads[params_.n_threads - 1] =
      std::thread(&GroundSegmentation::insertionThread, this, cloud, start_index, end_index);
  // Wait for threads to finish.
  for (auto it = threads.begin(); it != threads.end(); ++it) {
    it->join();
  }
}

在投影过程，

1. 计算每个Segment的角度。