函数注释很详细

  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 }