#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/cloud_viewer.h> typedef pcl::PointXYZ PointT; int main(int argc, char** argv) { // All the objects needed pcl::PCDReader reader; pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>); // 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; pcl::PCDWriter writer; pcl::ExtractIndices<PointT> extract;//点提取对象 pcl::ExtractIndices<pcl::Normal> extract_normals;//点提取对象 // Datasets pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);//滤波后点云 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); //建立一个直通滤器来去除杂散的 NaNs pcl::PassThrough<PointT> pass;//创建直通滤波器对象 pass.setInputCloud(cloud); pass.setFilterFieldName("z"); pass.setFilterLimits(0.0, 1.5); pass.filter(*cloud_filtered);//保存剩余的点到cloud_filtered; std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; // 法线估计，为后续的法线分割准备数据 pcl::NormalEstimation<PointT, pcl::Normal> ne;//法线估计对象 pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());//以kdtree作为索引方式 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);//存储输出数据集 //设置搜索时所用的搜索机制，参数tree指向搜索时所用的搜索对象，例如kd-tree, octree等对象。 ne.setSearchMethod(tree); ne.setInputCloud(cloud_filtered);//输入数据 ne.setKSearch(50);//参数 ne.compute(*cloud_normals); //设置分割所用的模型类型、方法和相关参数 pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;//点云分割对象,是利用采样一致性算法实现分割类 seg.setOptimizeCoefficients(true); //设置随机采样一致性所构造的几何模型的类型,定义为有条件限制的平面模型 seg.setModelType(pcl::SACMODEL_NORMAL_PLANE); //设置相对权重系数distance_weight，该权重与距离成正比，与角度成反比。 seg.setNormalDistanceWeight(0.1); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(100);//设置迭代次数的上限 // 该函数配合用户指定的模型，设置点到模型的距离阈值0.03，如果点到模型的距离不超过这个距离阂值， //认为该点为局内点，否则认为是局外点，被剔除。 seg.setDistanceThreshold(0.03); seg.setInputCloud(cloud_filtered); //设置输人点云的法线，normals为指向法线的指针。 seg.setInputNormals(cloud_normals); // 参数inliers是基于模型分割所得到的点云集合结果，model_ coefficients是得到的模型系数。 seg.segment(*inliers_plane, *coefficients_plane); std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl; // Extract the planar inliers from the input cloud extract.setInputCloud(cloud_filtered); extract.setIndices(inliers_plane);//对通过setInputCloud()和setIndices()共同指定的输入点云进行聚类分割 extract.setNegative(false); // Write the planar inliers to disk 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); 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::CloudViewer viewer("Cloud viewer"); viewer.showCloud(cloud_filtered); while (!viewer.wasStopped()) { } system("pause"); return (0); }