Linefit_ground_segmention文章梳理及代码阅读

2013年专门针对地面分割的文章：

Fast segmentation of 3D point clouds for ground vehicles

代码链接：

https://github.com/lorenwel/linefit_ground_segmentation

代码阅读：

目录

1、新建一个GroundSegmentation::GroundSegmentation类对象，struct GroundSegmentationParams结构体在头文件中赋初值，用于初始化类，包含了如下参数：

2、segment函数：输入为需要进行地面分割的点云以及标签容器

3、insertPoints函数

4、insertionThread函数

5、getLines函数：用于获取拟合直线

6、lineFitThread()函数：

7、fitSegmentLines函数

8、assignCluster(segmentation)函数

9、assignClusterThread函数

1、新建一个GroundSegmentation::GroundSegmentation类对象，struct GroundSegmentationParams结构体在头文件中赋初值，用于初始化类，包含了如下参数：

visualize(false),                 //是否可视化
r_min_square(0.3 * 0.3),          //设置点云离lidar最近的距离
r_max_square(20*20),              //设置点云离lidar最远的距离
n_bins(30),                       //将点云分为多少个环形
n_segments(180),                  //将点云分为多少个扇形
max_dist_to_line(0.15),           //对点到所有直线的距离，如果小于此值，则认为是地面，反之则不是
min_slope(0),                     //拟合直线的斜率最小值
max_slope(1),                     //拟合直线的斜率最大值，大于此值则直线不满足要求，去掉最后一点
n_threads(4),                     //4个线程
max_error_square(0.01),           //点到线性拟合的点的最大误差
long_threshold(2.0),              //判断为一根长线的阈值，大于此阈值则is_long_line=true
max_long_height(0.1),             //判断拟合直线的最后一点是否符合条件，是长线但是期望值与真实值大于此阈值则剔除
max_start_height(0.2),            //不符合长直线，但是最后拟合点和传感器所在高度差小于此范围，则加入拟合点
sensor_height(0.2),               //传感器高度差，与max_start_height是相对关系
line_search_angle(0.2)            //搜索步长小于此角度，则在邻域内找拟合直线。

2、segment函数：输入为需要进行地面分割的点云以及标签容器

该函数就是最重要的函数，主要分为3个子函数：1、insertPoints(cloud)函数：用于获取每个点云的环向索引和扇形索引；2、 getLines()函数：用于获取每一个segment中的拟合直线；3、assignCluster(segmentation)函数：执行分割任务，计算点到拟合直线的距离，将地面点云标签置为1。segment函数的注释代码如下：

输入为：待分割的点云和标签
void GroundSegmentation::segment(const PointCloud& cloud, std::vector<int>* segmentation) 
{
  std::cout << "Segmenting cloud with " << cloud.size() << " points...\n";
  // 计时
  std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();
  
  // segmention表示标签
  segmentation->clear();
  segmentation->resize(cloud.size(), 0);
  
  //bin_index_存储了每个点的bin索引和segment索引
  bin_index_.resize(cloud.size());
  
  //segment_coordinates_表示每个点在(d,z)上的索引
  segment_coordinates_.resize(cloud.size());
  
  // 获取了每个点云的bin索引和segment索引
  insertPoints(cloud);

  std::list<PointLine> lines;
  // 没用,不显示
  if (params_.visualize) {
    getLines(&lines);
  }
  // 执行这个
  else {
    getLines(NULL);
  }
  // 执行分割任务,输入就是标签
  assignCluster(segmentation);
  //不显示
  if (params_.visualize) 
  {
    // Visualize.
    PointCloud::Ptr obstacle_cloud(new PointCloud());
    // Get cloud of ground points.
    PointCloud::Ptr ground_cloud(new PointCloud());
    for (size_t i = 0; i < cloud.size(); ++i) {
      if (segmentation->at(i) == 1) ground_cloud->push_back(cloud[i]);
      else obstacle_cloud->push_back(cloud[i]);
    }
    PointCloud::Ptr min_cloud(new PointCloud());
    getMinZPointCloud(min_cloud.get());
    visualize(lines, min_cloud, ground_cloud, obstacle_cloud);
  }
  // 求取分割时间
  std::chrono::high_resolution_clock::time_point end =    std::chrono::high_resolution_clock::now();
  std::chrono::duration<double, std::milli> fp_ms = end - start;
  std::cout << "Done! Took " << fp_ms.count() << "ms\n";
}

3、insertPoints函数

void GroundSegmentation::insertPoints(const PointCloud& cloud) 
{
  // 线程数为4
  std