利用RanSac找到點雲中所有的平面

如題，這例直接上代碼。具體可以看代碼中的註釋。

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

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

		//創建分割對象 -- 檢測平面參數
		//pcl::search::KdTree<pcl::PointXYZ>::Ptr search(new pcl::search::KdTree<pcl::PointXYZ>);
		ModelCoefficients::Ptr coefficients(new ModelCoefficients); //存儲輸出的模型的係數
		PointIndices::Ptr inliers(new PointIndices); //存儲內點，使用的點
		SACSegmentation<PointXYZ> 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<float> 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<pcl::PointXYZ> extract;
		extract.setInputCloud(cloud);
		extract.setIndices(inliers);
		pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
		extract.filter(*output);//提取對於索引的點雲 內點
		std::cerr << "output point size : " << output->points.size() << std::endl;
		clusters.push_back(*inliers);

		// 移去平面局內點，提取剩餘點雲
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_other(new pcl::PointCloud<pcl::PointXYZ>);
		// *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);
	}
}