实现Ransac随机采样算法分割地面点云

如何利用RANSAC算法实现激光雷达的地面与非地面点云分割
利用激光雷达做感知输出首先要分割出地面点云以减少对障碍物聚类的影响。本文首先介绍RANSAC的基本原理，并依据RANSAC在ROS中实现对地面点云的分割。接着，引入PCL点云库，PCL点云库中有标准的RANSAC算法接口，我们可以通过调用它实现更加快速，稳定地滤除地面点云。
ransac是 random sample consensus的缩写，翻译过来是随机抽样一致的意思。它表示可以从一组包含“局外点”的数据集中，通过迭代的方式估计某个数学模型的参数。这是一种包含概率统计思想的“不确定算法”，它的每次迭代并不能保证获得一个正确合理的结果，这是带有概率性的，要想提高获得这种合理结果的概率，就需要增加迭代次数。

ransac的基本假设
整个数据集中同时包含好的点和不好的点，我们将它们分别称为局内点和局外点；
数据的分布可以通过某个数学模型来描述，而局内点就是可以适应该模型的点，局外点是不可以适应该模型的点；
随意给定一组点（可能是局内点也有可能是局外点），我们假设这组点都是局内点，它们满足某个数学模型，我们利用这个数学模型去估算其他的点，如果有足够多的点通过计算能被归类为假设的局内点，那么这个模型就足够合理。

ransac的算法流程
随机在数据集中选取几个点假设为局内点，点的个数选择要依据模型的特征确定，一般来说选择适应于模型的最小数据个数。比如说，要在一堆点中拟合一条直线，那就选取两个点，因为两点确定一条直线；如果要拟合一个平面，那你至少需要选取三个点且不共线。
计算适合这几个点的数学模型；
根据设定的阈值计算数据集中的其他点是否满足该数学模型，如果满足，记为该模型的局内点；
记录下该模型的局内点数量；
重复迭代多次，每次产生的模型要么因为局内点个数太少而被舍弃，要么就比现存模型更好而被选用；
满足迭代退出条件，退出循环，得到整个迭代过程中最合理的解。

如何确定迭代的退出条件？
用W表示随机抽到局内点的概率，用P表示置信度，这个参数表示RANSAC算法在运行后提供有用结果的期望概率。n表示计算模型参数需要选取的数据个数，k表示迭代次数，则

代码如下：

#include 
#include 
#include 
#include 
#include 
#include 
 
using namespace std;
int main()
{
     
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read("D:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection代码分解\\cloudRegion.pcd", *cloud);
 
	int num_points = cloud->points.size();
	//cout << num_points << endl; //2063
	std::unordered_set<int> inliersResult;
	
	int maxIterations = 40; //迭代次数
	while (maxIterations--)  // 
	{
     
		std::unordered_set<int> inliers;  //存放平面的内点，平面上的点也属于平面的内点
		//因为一开始定义inliers，内点并没有存在，所以随机在原始点云中随机选取了三个点作为点云的求取平面系数的三个点
		while (inliers.size() < 3)  //当内点小于3 就随机选取一个点 放入内点中 也就是需要利用到三个内点
		{
     
			inliers.insert(rand() % num_points);   //产生 0~num_points 中的一个随机数
		}
 
		// 需要至少三个点 才能找到地面
		float x1, y1, z1, x2, y2, z2, x3, y3, z3;
		auto itr = inliers.begin();  //auto 自动类型
		x1 = cloud->points[*itr].x;
		y1 = cloud->points[*itr].y;
		z1 = cloud->points[*itr].z;
		itr++;
		x2 = cloud->points[*itr].x;
		y2 = cloud->points[*itr].y;
		z2 = cloud->points[*itr].z;
		itr++;
		x3 = cloud->points[*itr].x;
		y3 = cloud->points[*itr].y;
		z3 = cloud->points[*itr].z;
 
		//计算平面系数
		float a, b, c, d, sqrt_abc;
		a = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
		b = (z2 - z1) * (x3 - x1) - (x2 - x1) * (z3 - z1);
		c = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);
		d = -(a * x1 + b * y1 + c * z1);
		sqrt_abc = sqrt(a * a + b * b + c * c);
 
		//分别计算这些点到平面的距离 
		for (int i = 0; i < num_points; i++)
		{
     
			if (inliers.count(i) > 0) //判断一下有没有内点
			{
      // that means: if the inlier in already exist, we dont need do anymore
				continue;
			}
			pcl::PointXYZ point = cloud->points[i];
			float x = point.x;
			float y = point.y;
			float z = point.z;
			float dist = fabs(a * x + b * y + c * z + d) / sqrt_abc; // calculate the distance between other points and plane
			float distanceTol = 0.3;
			if (dist < distanceTol) 
			{
     
				inliers.insert(i); //如果点云中的点 距离平面距离的点比较远 那么该点则视为内点
			}
			//将inliersResult 中的内容不断更新，因为地面的点一定是最多的，所以迭代40次 找到的inliersResult最大时，也就相当于找到了地面
			//inliersResult 中存储的也就是地面上的点云
			if (inliers.size() > inliersResult.size())
			{
     
				inliersResult = inliers;  
			}
		}
		//cout << inliers.size() << endl;
	}
	//迭代结束后，所有属于平面上的内点都存放在inliersResult中
	//std::unordered_set inliersResult;
	cout << inliersResult.size() << endl;  //1633
	
	//创建两片点云，一片存放地面，一片存放其他点
	pcl::PointCloud<pcl::PointXYZ>::Ptr out_plane(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr in_plane(new pcl::PointCloud<pcl::PointXYZ>);
	
	for (int i = 0; i < num_points; i++)
	{
     
		pcl::PointXYZ pt = cloud->points[i];
		if (inliersResult.count(i))
		{
     
			out_plane->points.push_back(pt);
		}
		else
		{
     
			in_plane->points.push_back(pt);
		}
	}
	
	/*pcl::PCDWriter writer;
	writer.write("C:\\Users\\Administrator\\Downloads\\求助\\求助\\tree-2-Rend.pcd", *cloud_filtered1);*/
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("显示窗口"));  //窗口显示点云
	viewer->addPointCloud(in_plane, "*cloud");
	viewer->resetCamera();		//相机点重置
	viewer->spin();
	system("pause");
	return (0);
}

参考:
https://blog.csdn.net/dengxiong_bright/article/details/102725848
https://blog.csdn.net/qinlele1994/article/details/106287529