使用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);
}