PCL：点云分割-圆柱体模型分割

#include <pcl/ModelCoefficients.h> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/filters/extract_indices.h> #include <pcl/filters/passthrough.h> #include <pcl/features/normal_3d.h> #include <pcl/sample_consensus/method_types.h> #include <pcl/sample_consensus/model_types.h> #include <pcl/segmentation/sac_segmentation.h> #include <pcl/visualization/pcl_visualizer.h> #pragma comment(lib,"User32.lib") #pragma comment(lib, "gdi32.lib") typedef pcl::PointXYZ PointT; int main(int argc, char** argv) { // All the objects needed pcl::PCDReader reader; //PCD文件读取对象 pcl::PassThrough<PointT> pass; //直通滤波对象 pcl::NormalEstimation<PointT, pcl::Normal> ne; //法线估计对象 pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; //分割对象 pcl::PCDWriter writer; //PCD文件读取对象 pcl::ExtractIndices<PointT> extract; //点提取对象 pcl::ExtractIndices<pcl::Normal> extract_normals; ///点提取对象 pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>()); // Datasets pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>); pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<PointT>::Ptr cloud_filtered2(new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>); pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices); // Read in the cloud data reader.read("table_scene_mug_stereo_textured.pcd", *cloud); std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl; // 直通滤波，将Z轴不在（0，1.5）范围的点过滤掉，将剩余的点存储到cloud_filtered对象中 pass.setInputCloud(cloud); pass.setFilterFieldName("z"); pass.setFilterLimits(0, 1.5); pass.filter(*cloud_filtered); std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; writer.write("直通滤波删除1.5以外的点.pcd", *cloud_filtered, false); // 过滤后的点云进行法线估计，为后续进行基于法线的分割准备数据 ne.setSearchMethod(tree); ne.setInputCloud(cloud_filtered); ne.setKSearch(50); ne.compute(*cloud_normals); writer.write("点云法线估计.pcd", *cloud_normals, false); // Create the segmentation object for the planar model and set all the parameters seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_NORMAL_PLANE); seg.setNormalDistanceWeight(0.1); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(100); seg.setDistanceThreshold(0.03); seg.setInputCloud(cloud_filtered); seg.setInputNormals(cloud_normals); //获取平面模型的系数和处在平面的内点 seg.segment(*inliers_plane, *coefficients_plane); std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl; // 从点云中抽取分割的处在平面上的点集 extract.setInputCloud(cloud_filtered); extract.setIndices(inliers_plane); extract.setNegative(false); // 存储分割得到的平面上的点到点云文件 pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>()); extract.filter(*cloud_plane); std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl; writer.write("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false); // Remove the planar inliers, extract the rest extract.setNegative(true); extract.filter(*cloud_filtered2); extract_normals.setNegative(true); extract_normals.setInputCloud(cloud_normals); extract_normals.setIndices(inliers_plane); extract_normals.filter(*cloud_normals2); // Create the segmentation object for cylinder segmentation and set all the parameters seg.setOptimizeCoefficients(true); //设置对估计模型优化 seg.setModelType(pcl::SACMODEL_CYLINDER); //设置分割模型为圆柱形 seg.setMethodType(pcl::SAC_RANSAC); //参数估计方法 seg.setNormalDistanceWeight(0.1); //设置表面法线权重系数 seg.setMaxIterations(10000); //设置迭代的最大次数10000 seg.setDistanceThreshold(0.05); //设置内点到模型的距离允许最大值 seg.setRadiusLimits(0, 0.1); //设置估计出的圆柱模型的半径的范围 seg.setInputCloud(cloud_filtered2); seg.setInputNormals(cloud_normals2); // Obtain the cylinder inliers and coefficients seg.segment(*inliers_cylinder, *coefficients_cylinder); std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl; // Write the cylinder inliers to disk extract.setInputCloud(cloud_filtered2); extract.setIndices(inliers_cylinder); extract.setNegative(false); pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>()); extract.filter(*cloud_cylinder); if (cloud_cylinder->points.empty()) std::cerr << "Can't find the cylindrical component." << std::endl; else { std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size() << " data points." << std::endl; writer.write("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false); } // 可视化部分 pcl::visualization::PCLVisualizer v0("segmention"); // 我们将要使用的颜色 float bckgr_gray_level = 0.0; // 黑色 float txt_gray_lvl = 1.0 - bckgr_gray_level; // 设置初始点云为白色 pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl);//赋予显示点云的颜色 v0.addPointCloud(cloud, cloud_in_color_h, "cloud"); // 可视化部分 pcl::visualization::PCLVisualizer viewer("segmention"); // 设置cloud_plane点云为红色 pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_plane, 0, 0, 255); viewer.addPointCloud(cloud_plane, cloud_tr_color_h, "cloud_plane"); // 设置cloud_cylinder点云为绿色 pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_cylinder, 0, 255, 0); viewer.addPointCloud(cloud_cylinder, cloud_icp_color_h, "cloud_cylinder"); 启动可视化 //v0.addCoordinateSystem(0.0); //v0.initCameraParameters(); //viewer.addCoordinateSystem(0.0); //viewer.initCameraParameters(); //等待直到可视化窗口关闭。 while (!viewer.wasStopped()) { v0.spinOnce(100); viewer.spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } return (0); }