cluster_extraction.cpp
Go to the documentation of this file.
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 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:22:46