利用RanSac找到点云中所有的平面

如题,这例直接上代码。具体可以看代码中的注释。

void get_plane(PointCloud::Ptr cloud, vector> &Coffis, vector  &clusters,int threshold){

	while (cloud->points.size() >= threshold){

		//创建分割对象 -- 检测平面参数
		//pcl::search::KdTree::Ptr search(new pcl::search::KdTree);
		ModelCoefficients::Ptr coefficients(new ModelCoefficients); //存储输出的模型的系数
		PointIndices::Ptr inliers(new PointIndices); //存储内点,使用的点
		SACSegmentation seg;
		//可选设置
		seg.setOptimizeCoefficients(true);
		//必须设置
		seg.setModelType(SACMODEL_PLANE); //设置模型类型,检测平面
		seg.setMethodType(SAC_RANSAC);      //设置方法【聚类或随机样本一致性】
		seg.setMaxIterations(1000);
		seg.setDistanceThreshold(0.01);
		//seg.setSamplesMaxDist(0.1, search);
		seg.setInputCloud(cloud);
		seg.segment(*inliers, *coefficients);    //分割操作
		//search->setInputCloud(cloud);
		if (inliers->indices.size() == 0){
			PCL_ERROR("Could not estimate a planar model for the given dataset.");
			return;
		}
		if (inliers->indices.size()<1000){
			break;
		}
		//平面参数
		vector tmp;
		tmp.push_back(coefficients->values[0]);
		tmp.push_back(coefficients->values[1]);
		tmp.push_back(coefficients->values[2]);
		tmp.push_back(coefficients->values[3]);
		Coffis.push_back(tmp);
		std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " "
			<< coefficients->values[2] << " " << coefficients->values[3] << std::endl;

		//提取平面
		pcl::ExtractIndices extract;
		extract.setInputCloud(cloud);
		extract.setIndices(inliers);
		pcl::PointCloud::Ptr output(new pcl::PointCloud);
		extract.filter(*output);//提取对于索引的点云 内点
		std::cerr << "output point size : " << output->points.size() << std::endl;
		clusters.push_back(*inliers);

		// 移去平面局内点,提取剩余点云
		pcl::PointCloud::Ptr cloud_other(new pcl::PointCloud);
		// *cloud_other = *cloud - *output;
		extract.setNegative(true);
		extract.filter(*cloud_other);
		std::cerr << "other point size : " << cloud_other->points.size() << std::endl;
		cloud = cloud_other;
		//D3_view(output, cloud_other, false);
	}
}

 

你可能感兴趣的:(Point,Cloud,pcl)