point_cloud_labeler.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/filter_indices.h>
00005 //#include <pcl/filters/voxel_grid.h>
00006 //#include <pcl/features/normal_3d.h>
00007 #include <pcl/search/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 #include <pcl/common/time.h>
00013 #include <pcl/console/parse.h>
00014 
00015 #include <vector>
00016 #include <limits>
00017 typedef typename pcl::PointXYZ PointIn;
00018 typedef typename pcl::PointXYZI PointOut;
00019 typedef pcl::PointCloud<PointIn> CloudIn;
00020 typedef typename CloudIn::Ptr CloudInPtr;
00021 typedef pcl::PointCloud<PointOut> CloudOut;
00022 typedef typename CloudOut::Ptr CloudOutPtr;
00023 
00024   struct CloudLabelPair {
00025     std::string name;
00026     int label;
00027   } ;
00028 
00029 int 
00030 main (int argc, char** argv)
00031 {
00032   //When do the scene-label pairs begin in argv table
00033   if (argc == 1)
00034     {
00035       pcl::console::print_info ("Syntax is: %s <-in scene_in.pcd> <-r radius> <-out scene_out.pcd> <-n subscenes in argv table>  <subscene1.pcd label1 ...> \n", argv[0]);
00036       exit(0);
00037     }
00038 
00039   double radius;
00040   pcl::console::parse_argument (argc, argv, "-r", radius);
00041 
00042   std::string in_cloud;
00043   pcl::console::parse_argument (argc, argv, "-in", in_cloud);
00044 
00045   std::string out_cloud;
00046   pcl::console::parse_argument (argc, argv, "-out", out_cloud);
00047 
00048   int subscene_argc;
00049   pcl::console::parse_argument (argc, argv, "-n", subscene_argc);
00050 
00051   std::vector<CloudLabelPair> all_clouds_and_labels;
00052   CloudLabelPair pair;
00053   for (int i = subscene_argc; i< argc; i=i+2)
00054     {
00055       pair.name = std::string(argv[i]);
00056       pair.label = atoi(argv[i+1]);
00057       all_clouds_and_labels.push_back(pair);
00058     }
00059 
00060   double start, stop;
00061   // Read in the full cloud
00062   pcl::PCDReader reader;
00063   CloudInPtr cloud (new CloudIn), query_cloud (new CloudIn);
00064   CloudOutPtr cloud_out (new CloudOut);
00065   reader.read (in_cloud, *cloud);
00066   pcl::copyPointCloud(*cloud, *cloud_out);
00067   for (uint i = 0; i < cloud_out->points.size(); i++)
00068     {
00069       cloud_out->points[i].intensity = -1;
00070     }
00071 
00072   // Creating the KdTree object for the search method of the extraction
00073   pcl::search::KdTree<PointIn>::Ptr tree (new  pcl::search::KdTree<PointIn>);
00074   std::vector< int > cloud_non_nan;
00075   std::cerr << "removing NaN points: " << cloud->points.size() << std::endl;
00076   pcl::removeNaNFromPointCloud  (*cloud, cloud_non_nan);
00077   std::cerr << "removed NaN points: " << cloud_non_nan.size() << std::endl;
00078   pcl::IndicesPtr cloud_non_nan_ind (new std::vector<int>);
00079   *cloud_non_nan_ind = cloud_non_nan;
00080   tree->setInputCloud (cloud, cloud_non_nan_ind);
00081   std::vector<int> kd_indices;
00082   std::vector<float> kd_distances;
00083   PointIn query;
00084   int knn = 0;
00085   double dist;
00086   int closest;
00087   //reading in and performing NN search for part clouds
00088   for (uint k = 0; k < all_clouds_and_labels.size(); k++)
00089     {
00090       reader.read (all_clouds_and_labels[k].name, *query_cloud);
00091       int label = all_clouds_and_labels[k].label;
00092       start = pcl::getTime ();
00093       std::cerr << "starting radiusSearch for label: " << label << " , query_cloud size: " << query_cloud->points.size() << std::endl;
00094       for (uint i = 0; i < query_cloud->points.size(); i++)
00095         {
00096           query = query_cloud->points[i];
00097           if( !pcl_isfinite( query.x ) ||
00098               !pcl_isfinite( query.y ) ||
00099               !pcl_isfinite( query.z ) )
00100             continue; 
00101           tree->radiusSearch (query, radius, kd_indices, kd_distances, knn);
00102           dist = std::numeric_limits<double>::max();
00103           closest = -1;
00104           if (kd_indices.size() != 0)
00105             {
00106               for (uint j = 0; j < kd_indices.size(); j++)
00107                 {
00108                   if (kd_distances[j] < dist)
00109                     {
00110                       dist = kd_distances[j];
00111                       closest = kd_indices[j];
00112                     }
00113                 }
00114             }
00115           if (closest != -1)
00116             {
00117               cloud_out->points[closest].intensity = label;
00118             }
00119         }
00120       stop = pcl::getTime ();
00121       std::cerr << "search query: " << stop - start << std::endl;
00122     }
00123 
00124   pcl::PCDWriter writer;
00125   writer.write (out_cloud.c_str(), *cloud_out, false); //*
00126 
00127   return (0);
00128 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_tools
Author(s): Nico Blodow, Zoltan-Csaba Marton, Dejan Pangercic
autogenerated on Thu May 23 2013 17:11:36