PCL点云平面分割

PCL点云平面分割

多平面分割方法封装

/** \brief 多平面分割
* \param[in] cloud_in 待分割点云
* \param[in] maxiter 最大迭代次数
* \param[in] dist 判断是否为平面内点的距离阈值
* \param[in] proportion 非平面点所占点云比例,[0,1]之间取值
* \param[out] out_plane_vect 分割后的平面模型点云集合
* \return 0成功，-1失败
*/
int g_multPlaneSeg(pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud_in, int maxiter, double dist,  float proportion, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& out_plane_vect)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::copyPointCloud(*cloud_in, *cloudXYZ);
	std::vector<int> mapping;
	pcl::removeNaNFromPointCloud(*cloudXYZ, *cloudXYZ, mapping);

	out_plane_vect.clear();
	//平面提取，获取平面方程
	pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setModelType(pcl::SACMODEL_PLANE);//平面模型		

	seg.setOptimizeCoefficients(true); //设置对估计的模型参数进行优化处理
	seg.setMethodType(pcl::SAC_RANSAC); // 设置用哪个随机参数估计方法,分割方法：随机采样法

	seg.setMaxIterations(maxiter);//设置最大迭代次数 
	seg.setDistanceThreshold(dist);//设置判断是否为模型内点的距离阈值

	size_t nr_points = cloudXYZ->points.size();

	while (cloudXYZ->points.size() > proportion * nr_points)
	{
		pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
		pcl::PointIndices::Ptr inliersIndices(new pcl::PointIndices());

		seg.setInputCloud(cloudXYZ);
		seg.segment(*inliersIndices, *coefficients);
		if (inliersIndices->indices.size() == 0)//没有提取出点
		{
			break;
		}
		pcl::PointCloud<pcl::PointXYZ>::Ptr  cloud_inliers(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr  cloud_outliers(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::ExtractIndices<pcl::PointXYZ> extract; //创建点云提取对象
		extract.setInputCloud(cloudXYZ);//设置输入点云
		extract.setIndices(inliersIndices);
		extract.setNegative(false); //设置提取内点而非外点
		extract.filter(*cloud_inliers);//提取输出存储到cloud_inliers
		extract.setNegative(true);//提取外点
		extract.filter(*cloud_outliers);
		*cloudXYZ = *cloud_outliers;
		out_plane_vect.push_back(cloud_inliers);
	}
	if (out_plane_vect.size() == 0)
	{
		return -1;
	}
	return 0;
}

调用如下

int main()
{
	std::string filename_ = "bunny.pcd";

	pcl::console::TicToc timecal;
	timecal.tic();
	pcl::PointCloud<pcl::PointXYZ>::Ptr model(new pcl::PointCloud<pcl::PointXYZ>());
	if (pcl::io::loadPCDFile(filename_, *model) < 0)
	{
		std::cout << "Error loading model cloud." << std::endl;
		return (-1);
	}
	std::cout << "加载点云成功.耗时：" << timecal.toc() << "ms" << std::endl;
	//速度慢降采样
	pcl::VoxelGrid<pcl::PointXYZ> vg;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	vg.setInputCloud(model);
	vg.setLeafSize(0.005f, 0.005f, 0.005f);
	vg.filter(*cloud_filtered);

	timecal.tic();
	std::cout << "开始提取平面" << std::endl;
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> outPlaneVect;
	g_multPlaneSeg(cloud_filtered, 10000, 0.02, 0.2, outPlaneVect);
	std::cout << "结束提取平面" << std::endl;
	std::cout << "提取平面个数:" << outPlaneVect.size() <<"。耗时："<< timecal.toc() << "ms" << std::endl;


	// visualization
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Viewer"));
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
	srand(time(nullptr));
	std::vector<unsigned char> color;
	for (size_t i = 0; i < outPlaneVect.size(); i++)
	{
		color.push_back(static_cast<unsigned char>(rand() % 256));
		color.push_back(static_cast<unsigned char>(rand() % 256));
		color.push_back(static_cast<unsigned char>(rand() % 256));
	}
	int color_index(0);
	for (size_t i = 0; i < outPlaneVect.size(); i++)
	{
		for (size_t j = 0; j < outPlaneVect[i]->points.size(); j++)
		{
			if (pcl_isnan(outPlaneVect[i]->points[j].x))
			{
				continue;
			}
			pcl::PointXYZRGB n_points;
			n_points.x = outPlaneVect[i]->points[j].x;
			n_points.y = outPlaneVect[i]->points[j].y;
			n_points.z = outPlaneVect[i]->points[j].z;
			n_points.r = color[3 * color_index];
			n_points.g = color[3 * color_index + 1];
			n_points.b = color[3 * color_index + 2];
			cloud_color->push_back(n_points);
		}
		color_index++;
	}
	viewer->addPointCloud(cloud_color);
	viewer->spin();

	system("pause");
	return 0;
}