apollo 路面检测分析

路面检测具体代码

路面检测 modules/perception/common/i_lib/pc/i_ground.cc

看百度的专利：[百度]一种地面检测方法、装置、电子设备、车辆及存储介质

bool PlaneFitGroundDetector::Detect(const float *point_cloud,
                                    float *height_above_ground,
                                    unsigned int nr_points,
                                    unsigned int nr_point_elements)
1
2
3
4

1 该函数中，依次建立精细网格和粗网格

  // setup the fine voxel grid
  if (!vg_fine_->SetS(point_cloud, nr_points, nr_point_elements)) {
    return false;
  }
  // setup the coarse voxel grid
  if (!vg_coarse_->SetS(point_cloud, nr_points, nr_point_elements)) {
    return false;
  }
1
2
3
4
5
6
7
8

在 Init()函数中定义格网大小

  // fine grid:
  vg_fine_ = new VoxelGridXY();
  if (vg_fine_ == nullptr) {
    return false;
  }
  if (!vg_fine_->Alloc(param_.nr_grids_fine, param_.nr_grids_fine,
                       -param_.roi_region_rad_x, param_.roi_region_rad_x,
                       -param_.roi_region_rad_y, param_.roi_region_rad_y,
                       -param_.roi_region_rad_z, param_.roi_region_rad_z)) {
    return false;
  }
  // coarse grid:
  vg_coarse_ = new VoxelGridXY();
  if (vg_coarse_ == nullptr) {
    return false;
  }
  if (!vg_coarse_->Alloc(param_.nr_grids_coarse, param_.nr_grids_coarse,
                         -param_.roi_region_rad_x, param_.roi_region_rad_x,
                         -param_.roi_region_rad_y, param_.roi_region_rad_y,
                         -param_.roi_region_rad_z, param_.roi_region_rad_z)) {
    return false;
  }

// 默认值：
  nr_grids_fine = 256;     // must be 2 and above
  nr_grids_coarse = 16;    // must be 2 and above
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

2 挑出候选点：Filter to generate plane fitting candidates

int nr_candis = Filter();

Filter内部如下：
  //  Filter plane fitting candidates
  // 对精细格网的每一行,生成候选平面
  for (r = 0; r < param_.nr_grids_fine; ++r) {
    nr_candis += FilterLine(r);
  }
1
2
3
4
5
6
7
8

再来看 FilterLine 定义：就是对格网每一行进行过滤，看看每行能够成为平面候选点的个数

int PlaneFitGroundDetector::FilterLine(unsigned int r) {
  int nr_candis = 0;
  unsigned int c = 0;
  const float *point_cloud = vg_fine_->const_data();
  unsigned int nr_points = vg_fine_->NrPoints();
  unsigned int nr_point_element = vg_fine_->NrPointElement();
  unsigned int begin = (r * param_.nr_grids_fine); // 每行的开始格网序号
  int parent = 0;
  // 这里对每一列找候选点
  for (c = 0; c < param_.nr_grids_fine; c++) {
    parent = map_fine_to_coarse_[begin + c];
    nr_candis +=
        FilterGrid((*vg_fine_)(r, c), point_cloud, &local_candis_[0][parent],
                   nr_points, nr_point_element);
  }
  return nr_candis;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

其中 mapfine_to_coarse定义：

// 倍数：精格网个数/粗格网个数
unsigned int sf = param_.nr_grids_fine / param_.nr_grids_coarse;
  // map of fine grid id to coarse id:
  map_fine_to_coarse_ = IAlloc(
      param_.nr_grids_fine * param_.nr_grids_fine);
  if (!map_fine_to_coarse_) {
    return false;
  }
  for (r = 0; r < param_.nr_grids_fine; ++r) {
    pr = r / sf; // 粗格网行号
    index = r * param_.nr_grids_fine;
    for (c = 0; c < param_.nr_grids_fine; ++c) {
      pc = c / sf; // 粗格网列号
      map_fine_to_coarse_[index + c] = pr * param_.nr_grids_coarse + pc; // 建立map映射
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

就是利用了精细格网与粗格网的个数，建立了格网序号的匹配

再来看 FilterGrid 函数

• 首先对于每一个体素网格内的点云数量进行统计，
  设置一个阈值nr_z_comp_candis：
  • 如果当前细粒度体素网格内的点云数量小于等于nr_z_comp_candis，留下所有的点云
  • 如果当前体素网格内的点云数量大于nr_z_comp_candis，随机留下nr_z_comp_candis个点云。
• 然后通过CompareZ()函数，判断该点能够成为候选点。

CompareZ() 函数中

设定一个 z 高度的阈值planefit_filter_threshold，遍历所有点云，将每个点云的高度 z 与其他点云的高度作差，如果差值的绝对值大于planefit_filter_threshold则nr_contradi加1。

最后看nr_contradi是否大于nr_z_comp_fail_threshold。大于则说明这个区域中大部分的点与当前点的高度相差过大，排除当前点。如果小于则说明这个区域中大部分的点与当前点的高度相差不大，留下当前点。

这样每个细粒度体素网格内的有效点就准备好了。合并到对应的粗粒度体素网格中就得到了粗粒度体素网格的有效点。

3 拟合平面：FitInOrder();

对于粗粒度体素网格，先拿到一个格子，找到它的邻居。FitGridWithNeighbors -> GetNeighbors

随机多次选取3个点算出一个平面，看这些平面距离所有的点的距离，只有在平面一定范围内的点的数量超过了阈值才是需要的平面。

由此可以得到几个平面。但这还不够，不能只关注自身还要关注全局，所以要计算自身与邻居的关系，看看我们的平面与邻居点的差距，大的就不要了。

  // generate plane hypothesis and vote
  for (int i = 0; i < param_.nr_ransac_iter_threshold; ++i) {
    IRandomSample(indices_trial, 3, nr_samples, &rseed);
    IScale3(indices_trial, dim_point_);
    ICopy3(pf_threeds_ + indices_trial[0], samples);
    ICopy3(pf_threeds_ + indices_trial[1], samples + 3);
    ICopy3(pf_threeds_ + indices_trial[2], samples + 6);
    IPlaneFitDestroyed(samples, hypothesis[i].params);
    // check if the plane hypothesis has valid geometry
    if (hypothesis[i].GetDegreeNormalToZ() > param_.planefit_orien_threshold) {
      continue;
    }
    // iterate samples and check if the point to plane distance is below
    // threshold
    psrc = pf_threeds_;
    nr_inliers = 0;
    for (int j = 0; j < nr_samples; ++j) {
      ptp_dist = IPlaneToPointDistanceWUnitNorm(hypothesis[i].params, psrc);
      if (ptp_dist < dist_thre) {
        nr_inliers++;
      }
      psrc += dim_point_;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

再关注平面与邻居平面的夹角，大的就不要了，排选出最优平面。
再看看所的平面是否符合要求。

 angle = CalculateAngleDist(hypothesis[i], neighbors);
1

Smooth plane using neighborhood information:

  AINFO << "nr_smooth_iter: " << param_.nr_smooth_iter;
  for (int iter = 0; iter < param_.nr_smooth_iter; ++iter) {
    int nr_grid = Smooth();
    AINFO << "the " << iter << "th nr_grid is: " << nr_grid;
  }
1
2
3
4
5

4 计算点到平面的距离，去除距离地面近的点：

  // compute point to ground distance
  ComputeSignedGroundHeight(point_cloud, height_above_ground, nr_points, -nr_point_elements);
1
2

Reference

知乎：Apollo 感知算法地面滤除代码详解