基于RANSAC的激光点云分割

Lidar系列文章

传感器融合是将多个传感器采集的数据进行融合处理，以更好感知周围环境；这里首先介绍激光雷达的相关内容，包括激光雷达基本介绍（本节内容），激光点云数据处理方法（点云数据显示，点云分割，点云聚类，障碍物识别实例）等。

系列文章目录

1. 激光雷达基本介绍
2. 激光点云数据显示
3. 基于RANSAC的激光点云分割
4. 点云分割入门级实例学习
5. 激光点云目标物聚类
6. 基于PCL实现欧式聚类提取
7. 激光雷达障碍物识别

---

RANSAC随机抽样一致简介

RANSAC（RANdom SAmple Consensus）随机抽样一致，最早于1981年由Fischler和Bolles提出，基本思想是根据一组包含噪声、外点的各种缺陷的样本数据集中，估计出数据的数学模型参数，并同时得到有效样本数据。

RANSAC从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。每次迭代时选取一个数据子集（线：2个数据点，面：三个数据点）并对其进行拟合，计算剩余点到拟合后的模型（线/面）的距离，包含最多数量的“局内点”（距离模型一定范围内的点）或最小的噪声作为最佳模型。

类似的RANSAC方法也包括先对数据集进行采样（如20%数据），对其进行拟合并计算偏差，偏差最小的为最佳模型。这种方法的优势是每次迭代中不用考虑所有点。

实例-使用RANSAC拟合直线

对于平面中的点，RANSAC方法拟合直线的主要步骤如下：

1. 每次迭代中从数据集中随机采样2个点；
2. 基于两个采样点拟合一条直线；
3. 计算每个点到拟合直线的距离，容差内的点为内点。
4. 每次迭代过程中跟踪拟合的直线，保留内点最多的拟合直线，即为最佳的模型。

下面将以RANSAC拟合2D点云中的直线进行介绍。下图是对数据集进行采样的子集。

直线公式：
$$Ax+By+C=0$$
设其经过点1（x1, y1）和点2（x2, y2），则经过这两点的直线公式为：
$$(y1-y2)x+(x2-x1)y+(x1\ast y2-x2\ast y1)=0$$
平面内点（x，y）到直线的距离d：
$$d=\mid Ax+By+C\mid /sqrt(A^2+B^2)$$

std::unordered_set<int> Ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol)
{
   
	std::unordered_set<int> inliersResult;
	srand(time(NULL)); //srand() generate a random seed associate with current time.
	
	// TODO: Fill in this function
	// For max iterations 
	// Randomly sample subset and fit line
	// Measure distance between every point and fitted line
	// If distance is smaller than threshold count it as inlier
	// Return indicies of inliers from fitted line with most inliers

  // Time segmentation process
  auto startTime = std::chrono::steady_clock::now();

  while(maxIterations--)// For max iterations 
  {
   
    //Randomly pick two points;
    std::unordered_set<int> inliers;
    while(inliers.size()<2)
      inliers.insert(rand()%(cloud->points.size()));

    float x1, y1, x2, y2;
    auto itr = inliers.begin();
    x1 = cloud->points[*itr].x;
    y1 = cloud->points[*itr].y;
    itr++;
    x2 = cloud->points[*itr].x;
    y2 = cloud->points[*itr].y;
    //Line formula ax + by + c = 0
    float a = y1 - y2;
    float b = x2 - x1;
    float c = x1*y2 - x2*y1;

    // Measure distance between every point and fitted line
    // If distance is smaller than threshold count it as inlier
    for(int index=0; index < cloud->points.size(); index++)
    {
   
      //discard previous two points
      if(inliers.count(index)>0)
        continue;
      
      pcl::PointXYZ point = cloud->points[index];
      float x3 = point.x;
      float y3