【激光雷达点云障碍物检测】（二）实现Ransac随机采样算法分割地面点云

真的是认真阅读大神的代码真的可以学到好多知识~~

本部分对滤波后的点云分割出地面，以便后续将地面上的点云在进行聚类分割检测。

分割效果如下：

 

代码如下：

#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <unordered_set>

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<int> 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);
}