RANSAC分割平面

使用PCL 进行RANSAC分割

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>


#define enable_outpoint false
#define name_save       "seave.pcd"

//面提取算法


using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
typedef pcl::PointXYZ PointT;
typedef pcl::PointXYZRGBA Pointcolor;
//设置显示模块
int vp_1, vp_2;
pcl::visualization::PCLVisualizer *p;


int main (int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr my_table_cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr my_table_voxel_process_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read<pcl::PointXYZ>("./sum.pcd",*my_table_cloud);
	//由于原有的table点云数据量太大，进行降采样
	pcl::VoxelGrid<pcl::PointXYZ> my_voxel_filter;
	my_voxel_filter.setInputCloud(my_table_cloud);
	my_voxel_filter.setLeafSize(0.01f,0.01f,0.01f);
	my_voxel_filter.filter(*my_table_voxel_process_cloud);

	//
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr indices(new pcl::PointIndices());
	pcl::SACSegmentation<pcl::PointXYZ> sacseg;
	sacseg.setOptimizeCoefficients(true);
	sacseg.setModelType(pcl::SACMODEL_PLANE);
	sacseg.setMethodType(pcl::SAC_RANSAC);
	sacseg.setMaxIterations(100);//最大迭代次数
	sacseg.setDistanceThreshold(0.1);//距离阈值
	sacseg.setInputCloud(my_table_voxel_process_cloud);
	sacseg.segment(*indices,*coefficients);

	if (indices->indices.size() == 0)
	{
		std::cerr<<"Could not estimate a planar model for the given dataset."<<std::endl;
	}
	pcl::ExtractIndices<pcl::PointXYZ> my_extract_indices;
	my_extract_indices.setInputCloud(my_table_voxel_process_cloud);
	my_extract_indices.setIndices(indices);
	my_extract_indices.setNegative(true);
	my_extract_indices.filter(*cloud_p);

	//用于输出最后提取的点集
	if (enable_outpoint == true)
		for (size_t i=0;i<cloud_p->points.size();++i)
		{
			std::cout<<cloud_p->points[i].x<<" "<<cloud_p->points[i].y<<" "<<cloud_p->points[i].z<<" "<<std::endl;
		}
	//用于输出模型的系数
	std::cout<<"模型系数为： "<<coefficients->values[0]<<" "<<coefficients->values[1]<<" "<<coefficients->values[2]<<" "<<coefficients->values[3]<<std::endl;

	//保存clould_p
	pcl::io::savePCDFileASCII (name_save, *cloud_p);


	p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");
	p->createViewPort (0.0, 0, 0.5, 1.0, vp_1);
	p->createViewPort (0.5, 0, 1.0, 1.0, vp_2);


	//显示左边窗口---分割后的图形
	p->removePointCloud ("segment");
	PointCloudColorHandlerCustom<PointT> tgt_h (my_table_cloud, 255, 0, 0);
	p->addPointCloud (my_table_cloud, tgt_h, "vp1_target", vp_1);
	//显示右边窗口
	p->removePointCloud ("vp2_target");
	PointCloudColorHandlerCustom<PointT> tgt_h2 (cloud_p, 0, 255, 0);
	p->addPointCloud (cloud_p, tgt_h2, "vp2_target", vp_2);
	p-> spin();




	return (0);
}