00001 #include <pcl/ModelCoefficients.h> 00002 #include <pcl/point_types.h> 00003 #include <pcl/io/pcd_io.h> 00004 #include <pcl/filters/extract_indices.h> 00005 #include <pcl/filters/voxel_grid.h> 00006 #include <pcl/features/normal_3d.h> 00007 #include <pcl/kdtree/kdtree.h> 00008 #include <pcl/sample_consensus/method_types.h> 00009 #include <pcl/sample_consensus/model_types.h> 00010 #include <pcl/segmentation/sac_segmentation.h> 00011 #include <pcl/segmentation/extract_clusters.h> 00012 00013 00014 int 00015 main (int argc, char** argv) 00016 { 00017 // Read in the cloud data 00018 pcl::PCDReader reader; 00019 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); 00020 reader.read ("table_scene_lms400.pcd", *cloud); 00021 std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* 00022 00023 // Create the filtering object: downsample the dataset using a leaf size of 1cm 00024 pcl::VoxelGrid<pcl::PointXYZ> vg; 00025 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); 00026 vg.setInputCloud (cloud); 00027 vg.setLeafSize (0.01f, 0.01f, 0.01f); 00028 vg.filter (*cloud_filtered); 00029 std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* 00030 00031 // Create the segmentation object for the planar model and set all the parameters 00032 pcl::SACSegmentation<pcl::PointXYZ> seg; 00033 pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 00034 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 00035 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); 00036 pcl::PCDWriter writer; 00037 seg.setOptimizeCoefficients (true); 00038 seg.setModelType (pcl::SACMODEL_PLANE); 00039 seg.setMethodType (pcl::SAC_RANSAC); 00040 seg.setMaxIterations (100); 00041 seg.setDistanceThreshold (0.02); 00042 00043 int i=0, nr_points = (int) cloud_filtered->points.size (); 00044 while (cloud_filtered->points.size () > 0.3 * nr_points) 00045 { 00046 // Segment the largest planar component from the remaining cloud 00047 seg.setInputCloud (cloud_filtered); 00048 seg.segment (*inliers, *coefficients); 00049 if (inliers->indices.size () == 0) 00050 { 00051 std::cout << "Could not estimate a planar model for the given dataset." << std::endl; 00052 break; 00053 } 00054 00055 // Extract the planar inliers from the input cloud 00056 pcl::ExtractIndices<pcl::PointXYZ> extract; 00057 extract.setInputCloud (cloud_filtered); 00058 extract.setIndices (inliers); 00059 extract.setNegative (false); 00060 00061 // Get the points associated with the planar surface 00062 extract.filter (*cloud_plane); 00063 std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; 00064 00065 // Remove the planar inliers, extract the rest 00066 extract.setNegative (true); 00067 extract.filter (*cloud_f); 00068 *cloud_filtered = *cloud_f; 00069 } 00070 00071 // Creating the KdTree object for the search method of the extraction 00072 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); 00073 tree->setInputCloud (cloud_filtered); 00074 00075 std::vector<pcl::PointIndices> cluster_indices; 00076 pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; 00077 ec.setClusterTolerance (0.02); // 2cm 00078 ec.setMinClusterSize (100); 00079 ec.setMaxClusterSize (25000); 00080 ec.setSearchMethod (tree); 00081 ec.setInputCloud (cloud_filtered); 00082 ec.extract (cluster_indices); 00083 00084 int j = 0; 00085 for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) 00086 { 00087 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); 00088 for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) 00089 cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //* 00090 cloud_cluster->width = cloud_cluster->points.size (); 00091 cloud_cluster->height = 1; 00092 cloud_cluster->is_dense = true; 00093 00094 std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; 00095 std::stringstream ss; 00096 ss << "cloud_cluster_" << j << ".pcd"; 00097 writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* 00098 j++; 00099 } 00100 00101 return (0); 00102 }