函数注释很详细
1 #include <pcl/ModelCoefficients.h> 2 #include <pcl/io/pcd_io.h> 3 #include <pcl/point_types.h> 4 #include <pcl/filters/extract_indices.h> 5 #include <pcl/filters/passthrough.h> 6 #include <pcl/features/normal_3d.h> 7 #include <pcl/sample_consensus/method_types.h> 8 #include <pcl/sample_consensus/model_types.h> 9 #include <pcl/segmentation/sac_segmentation.h>//基于采样一致性分割的类的头文件 10 #include <pcl/visualization/pcl_visualizer.h> 11 12 typedef pcl::PointXYZ PointT; 13 14 int 15 main(int argc, char** argv) 16 { 17 // ************设置所需对象读入点云数据 18 pcl::PCDReader reader; //PCD文件读取对象 19 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>); 20 reader.read("table_scene_mug_stereo_textured.pcd", *cloud); 21 std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl; 22 23 // ***********直通滤波，将Z轴不在（0，1.5）范围的点过滤掉，将剩余的点存储到cloud_filtered对象中 24 pcl::PassThrough<PointT> pass; //直通滤波对象 25 pass.setInputCloud(cloud); 26 pass.setFilterFieldName("z"); 27 pass.setFilterLimits(0,1.5); 28 pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>); 29 pass.filter(*cloud_filtered); 30 std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; 31 32 // ************点云进行法线估计，为后续进行基于法线的分割准备数据 33 pcl::NormalEstimation<PointT, pcl::Normal> ne; //法线估计对象 34 pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>()); 35 ne.setSearchMethod(tree);//设置内部算法实现时所用的搜索对象，tree为指向kdtree或者octree对应的指针 36 ne.setInputCloud(cloud_filtered); 37 ne.setKSearch(50);// 设置K近邻搜索时所用的K参数 38 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>); 39 ne.compute(*cloud_normals);//计算特征值 40 cout << cloud_normals->points.size(); 41 // ****************为平面模型创建分割对象并设置所有参数 42 pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; //分割对象 43 pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients); 44 pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices); 45 seg.setOptimizeCoefficients(true); 46 seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);//设置模型类型 47 seg.setMethodType(pcl::SAC_RANSAC);//设置随机采样一致性方法类型 48 seg.setNormalDistanceWeight(0.1); // 法线信息权重 49 seg.setMethodType(pcl::SAC_RANSAC);//随机采样一致性算法 50 seg.setMaxIterations(100); //最大迭代次数 51 seg.setDistanceThreshold(0.03); //设置内点到模型的距离允许最大值 52 seg.setInputCloud(cloud_filtered); //输入点云 53 seg.setInputNormals(cloud_normals);//设置输人点云的法线，normals为指向法线的指针。 54 seg.segment(*inliers_plane, *coefficients_plane);//存储分割结果到点几何inliers_plane及存储平面模型的系数coefficients_plane 55 std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl; 56 57 // *************从点云中抽取分割的处在平面上的点集 58 pcl::ExtractIndices<PointT> extract; //// 创建滤波器对象（点提取对象） 59 pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>()); 60 pcl::PointCloud<PointT>::Ptr cloud_filtered2(new pcl::PointCloud<PointT>); 61 extract.setInputCloud(cloud_filtered); 62 extract.setIndices(inliers_plane);//设置分割后的点内为需要提取的点集 63 extract.setNegative(false);//设置提取点内而非点外。 64 extract.filter(*cloud_plane);// 存储分割得到的平面上的点到点云文件 65 extract.setNegative(true);//设置提取点外而非点内。 66 extract.filter(*cloud_filtered2);//提取输出并储存到cloud_filterd2 67 std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl; 68 69 // ************Remove the planar inliers, extract the rest 70 pcl::ExtractIndices<pcl::Normal> extract_normals; //点提取对象 71 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>); 72 extract_normals.setInputCloud(cloud_normals); 73 extract_normals.setIndices(inliers_plane); 74 extract_normals.setNegative(true); 75 extract_normals.filter(*cloud_normals2); 76 77 // **************创建圆柱体的分割对象并设置所有参数 78 seg.setOptimizeCoefficients(true); //设置对估计模型优化 79 seg.setModelType(pcl::SACMODEL_CYLINDER); //设置分割模型为圆柱形 80 seg.setMethodType(pcl::SAC_RANSAC); //参数估计方法 81 seg.setNormalDistanceWeight(0.1); //设置表面法线权重系数 82 seg.setMaxIterations(10000); //设置迭代的最大次数10000 83 seg.setDistanceThreshold(0.05); //设置内点到模型的距离允许最大值 84 seg.setRadiusLimits(0, 0.1); //设置估计出的圆柱模型的半径的范围 85 seg.setInputCloud(cloud_filtered2); 86 seg.setInputNormals(cloud_normals2);//设置输人点云的法线，normals为指向法线的指针。 87 // Obtain the cylinder inliers and coefficients 88 pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients); 89 seg.segment(*inliers_cylinder, *coefficients_cylinder); 90 std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl; 91 92 // Write the cylinder inliers to disk 93 extract.setInputCloud(cloud_filtered2); 94 extract.setIndices(inliers_cylinder); 95 extract.setNegative(false); 96 pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>()); 97 extract.filter(*cloud_cylinder); 98 if (cloud_cylinder->points.empty()) 99 std::cerr << "Can't find the cylindrical component." << std::endl; 100 else 101 { 102 std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size() << " data points." << std::endl; 103 //writer.write("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false); 104 } 105 106 107 108 // 可视化部分 109 pcl::visualization::PCLVisualizer v0("segmention"); 110 // 我们将要使用的颜色 111 float bckgr_gray_level = 0.0; // 黑色 112 float txt_gray_lvl = 1.0 - bckgr_gray_level; 113 114 // 设置初始点云为白色 115 pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, 116 (int)255 * txt_gray_lvl);//赋予显示点云的颜色 117 v0.addPointCloud(cloud, cloud_in_color_h, "cloud"); 118 119 120 // 可视化部分 121 pcl::visualization::PCLVisualizer viewer("segmention"); 122 123 // 设置cloud_plane点云为红色 124 pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_plane, 0, 0, 255); 125 viewer.addPointCloud(cloud_plane, cloud_tr_color_h, "cloud_plane"); 126 127 // 设置cloud_cylinder点云为绿色 128 pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_cylinder, 0, 255, 0); 129 viewer.addPointCloud(cloud_cylinder, cloud_icp_color_h, "cloud_cylinder"); 130 131 // 启动可视化,可以取消 132 v0.addCoordinateSystem(0.0); 133 v0.initCameraParameters(); 134 viewer.addCoordinateSystem(0.0); 135 viewer.initCameraParameters(); 136 137 //等待直到可视化窗口关闭。 138 while (!viewer.wasStopped()) 139 { 140 v0.spinOnce(100); 141 viewer.spinOnce(100); 142 boost::this_thread::sleep(boost::posix_time::microseconds(100000)); 143 } 144 145 return (0); 146 }
