欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

PCL点云平面分割

程序员文章站 2024-03-25 10:53:46
...

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;
}
相关标签: PCL