don_segmentation.cpp
Go to the documentation of this file.
00001 
00008 #include <string>
00009 
00010 #include <pcl/point_types.h>
00011 #include <pcl/io/pcd_io.h>
00012 #include <pcl/search/organized.h>
00013 #include <pcl/search/kdtree.h>
00014 #include <pcl/features/normal_3d_omp.h>
00015 #include <pcl/filters/conditional_removal.h>
00016 #include <pcl/segmentation/extract_clusters.h>
00017 
00018 #include <pcl/features/don.h>
00019 
00020 using namespace pcl;
00021 using namespace std;
00022 
00023 int
00024 main (int argc, char *argv[])
00025 {
00027   double scale1;
00028 
00030   double scale2;
00031 
00033   double threshold;
00034 
00036   double segradius;
00037 
00038   if (argc < 6)
00039   {
00040     cerr << "usage: " << argv[0] << " inputfile smallscale largescale threshold segradius" << endl;
00041     exit (EXIT_FAILURE);
00042   }
00043 
00045   string infile = argv[1];
00047   istringstream (argv[2]) >> scale1;
00049   istringstream (argv[3]) >> scale2;
00050   istringstream (argv[4]) >> threshold;   // threshold for DoN magnitude
00051   istringstream (argv[5]) >> segradius;   // threshold for radius segmentation
00052 
00053   // Load cloud in blob format
00054   pcl::PCLPointCloud2 blob;
00055   pcl::io::loadPCDFile (infile.c_str (), blob);
00056   pcl::PointCloud<PointXYZRGB>::Ptr cloud (new pcl::PointCloud<PointXYZRGB>);
00057   pcl::fromPCLPointCloud2 (blob, *cloud);
00058 
00059   // Create a search tree, use KDTreee for non-organized data.
00060   pcl::search::Search<PointXYZRGB>::Ptr tree;
00061   if (cloud->isOrganized ())
00062   {
00063     tree.reset (new pcl::search::OrganizedNeighbor<PointXYZRGB> ());
00064   }
00065   else
00066   {
00067     tree.reset (new pcl::search::KdTree<PointXYZRGB> (false));
00068   }
00069 
00070   // Set the input pointcloud for the search tree
00071   tree->setInputCloud (cloud);
00072 
00073   if (scale1 >= scale2)
00074   {
00075     cerr << "Error: Large scale must be > small scale!" << endl;
00076     exit (EXIT_FAILURE);
00077   }
00078 
00079   // Compute normals using both small and large scales at each point
00080   pcl::NormalEstimationOMP<PointXYZRGB, PointNormal> ne;
00081   ne.setInputCloud (cloud);
00082   ne.setSearchMethod (tree);
00083 
00088   ne.setViewPoint (std::numeric_limits<float>::max (), std::numeric_limits<float>::max (), std::numeric_limits<float>::max ());
00089 
00090   // calculate normals with the small scale
00091   cout << "Calculating normals for scale..." << scale1 << endl;
00092   pcl::PointCloud<PointNormal>::Ptr normals_small_scale (new pcl::PointCloud<PointNormal>);
00093 
00094   ne.setRadiusSearch (scale1);
00095   ne.compute (*normals_small_scale);
00096 
00097   // calculate normals with the large scale
00098   cout << "Calculating normals for scale..." << scale2 << endl;
00099   pcl::PointCloud<PointNormal>::Ptr normals_large_scale (new pcl::PointCloud<PointNormal>);
00100 
00101   ne.setRadiusSearch (scale2);
00102   ne.compute (*normals_large_scale);
00103 
00104   // Create output cloud for DoN results
00105   PointCloud<PointNormal>::Ptr doncloud (new pcl::PointCloud<PointNormal>);
00106   copyPointCloud<PointXYZRGB, PointNormal>(*cloud, *doncloud);
00107 
00108   cout << "Calculating DoN... " << endl;
00109   // Create DoN operator
00110   pcl::DifferenceOfNormalsEstimation<PointXYZRGB, PointNormal, PointNormal> don;
00111   don.setInputCloud (cloud);
00112   don.setNormalScaleLarge (normals_large_scale);
00113   don.setNormalScaleSmall (normals_small_scale);
00114 
00115   if (!don.initCompute ())
00116   {
00117     std::cerr << "Error: Could not intialize DoN feature operator" << std::endl;
00118     exit (EXIT_FAILURE);
00119   }
00120 
00121   // Compute DoN
00122   don.computeFeature (*doncloud);
00123 
00124   // Save DoN features
00125   pcl::PCDWriter writer;
00126   writer.write<pcl::PointNormal> ("don.pcd", *doncloud, false); 
00127 
00128   // Filter by magnitude
00129   cout << "Filtering out DoN mag <= " << threshold << "..." << endl;
00130 
00131   // Build the condition for filtering
00132   pcl::ConditionOr<PointNormal>::Ptr range_cond (
00133     new pcl::ConditionOr<PointNormal> ()
00134     );
00135   range_cond->addComparison (pcl::FieldComparison<PointNormal>::ConstPtr (
00136                                new pcl::FieldComparison<PointNormal> ("curvature", pcl::ComparisonOps::GT, threshold))
00137                              );
00138   // Build the filter
00139   pcl::ConditionalRemoval<PointNormal> condrem (range_cond);
00140   condrem.setInputCloud (doncloud);
00141 
00142   pcl::PointCloud<PointNormal>::Ptr doncloud_filtered (new pcl::PointCloud<PointNormal>);
00143 
00144   // Apply filter
00145   condrem.filter (*doncloud_filtered);
00146 
00147   doncloud = doncloud_filtered;
00148 
00149   // Save filtered output
00150   std::cout << "Filtered Pointcloud: " << doncloud->points.size () << " data points." << std::endl;
00151 
00152   writer.write<pcl::PointNormal> ("don_filtered.pcd", *doncloud, false); 
00153 
00154   // Filter by magnitude
00155   cout << "Clustering using EuclideanClusterExtraction with tolerance <= " << segradius << "..." << endl;
00156 
00157   pcl::search::KdTree<PointNormal>::Ptr segtree (new pcl::search::KdTree<PointNormal>);
00158   segtree->setInputCloud (doncloud);
00159 
00160   std::vector<pcl::PointIndices> cluster_indices;
00161   pcl::EuclideanClusterExtraction<PointNormal> ec;
00162 
00163   ec.setClusterTolerance (segradius);
00164   ec.setMinClusterSize (50);
00165   ec.setMaxClusterSize (100000);
00166   ec.setSearchMethod (segtree);
00167   ec.setInputCloud (doncloud);
00168   ec.extract (cluster_indices);
00169 
00170   int j = 0;
00171   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it, j++)
00172   {
00173     pcl::PointCloud<PointNormal>::Ptr cloud_cluster_don (new pcl::PointCloud<PointNormal>);
00174     for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
00175     {
00176       cloud_cluster_don->points.push_back (doncloud->points[*pit]);
00177     }
00178 
00179     cloud_cluster_don->width = int (cloud_cluster_don->points.size ());
00180     cloud_cluster_don->height = 1;
00181     cloud_cluster_don->is_dense = true;
00182 
00183     //Save cluster
00184     cout << "PointCloud representing the Cluster: " << cloud_cluster_don->points.size () << " data points." << std::endl;
00185     stringstream ss;
00186     ss << "don_cluster_" << j << ".pcd";
00187     writer.write<pcl::PointNormal> (ss.str (), *cloud_cluster_don, false);
00188   }
00189 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:25