import_pcd.cpp
Go to the documentation of this file.
00001 #include <unistd.h>
00002 #include <iostream>
00003 #include <argp.h>
00004 
00005 #include <pcl/io/pcd_io.h>
00006 #include <pcl/pcl_base.h>
00007 #include <pcl/point_types.h>
00008 #include <pcl/filters/passthrough.h>
00009 
00010 #include <boost/algorithm/string.hpp>
00011 
00012 #include <megatree/common.h>
00013 #include <megatree/megatree.h>
00014 #include <megatree/storage_factory.h>
00015 #include <megatree/tree_functions.h>
00016 
00017 #include <fstream>
00018 #include <iostream>
00019 
00020 using namespace megatree;
00021 
00022 static unsigned long total_points = 0;
00023 static Tictoc overall_timer;
00024 
00025 template <class T>
00026 boost::shared_ptr<pcl::PointCloud<T> > removeNan(boost::shared_ptr<pcl::PointCloud<T> > in)
00027 {
00028   boost::shared_ptr<pcl::PointCloud<T> > out(new pcl::PointCloud<T>);
00029   pcl::PassThrough<T> pass_filter; 
00030   pass_filter.setInputCloud(in);
00031   pass_filter.setFilterFieldName("z");
00032   pass_filter.setFilterLimits(1e-5, 9999999);
00033   pass_filter.filter(*out);
00034   return out;
00035 }
00036 
00037 void importPCD(MegaTree &tree, const boost::filesystem::path &path, unsigned long max_intensity, unsigned int *skip = NULL)
00038 {
00039   //Load the point cloud in blob form so we can check fields
00040   sensor_msgs::PointCloud2 cloud_blob;
00041   pcl::io::loadPCDFile(path.string(), cloud_blob);
00042 
00043   //we need to check what fields are available
00044   int rgb_index = pcl::getFieldIndex(cloud_blob, "rgb");
00045   int intensity_index = pcl::getFieldIndex(cloud_blob, "intensity");
00046 
00047   //Because of the way PCL does types, this was the easiest thing I could think of to handle
00048   //different kinds of clouds. Probably not the best, but it'll work
00049   boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > xyz_cloud;
00050   boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > color_cloud;
00051   boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > intensity_cloud;
00052 
00053   //load the cloud in color if the rgb field is available
00054   if(rgb_index != -1)
00055   {
00056     color_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
00057     pcl::fromROSMsg(cloud_blob, *color_cloud);
00058     color_cloud = removeNan<pcl::PointXYZRGB>(color_cloud);
00059 
00060   }
00061   //otherwise, if intensity is available load the cloud that way
00062   else if(intensity_index != 1)
00063   {
00064     intensity_cloud.reset(new pcl::PointCloud<pcl::PointXYZI>);
00065     pcl::fromROSMsg(cloud_blob, *intensity_cloud);
00066     intensity_cloud = removeNan<pcl::PointXYZI>(intensity_cloud);
00067   }
00068   else
00069   {
00070     xyz_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
00071     pcl::fromROSMsg(cloud_blob, *xyz_cloud);
00072     xyz_cloud = removeNan<pcl::PointXYZ>(xyz_cloud);
00073   }
00074 
00075   // Adds each point into the tree
00076   std::vector<double> point(3, 0.0);
00077   std::vector<double> color(3, 0.0);
00078   for (unsigned int i = 0; i < cloud_blob.width * cloud_blob.height; i++)
00079   {
00080     if (i % 100000 == 0)
00081       printf("Adding point %u\n", i);
00082 
00083     if(color_cloud)
00084     {
00085       point[0] = color_cloud->points[i].x;
00086       point[1] = color_cloud->points[i].y;
00087       point[2] = color_cloud->points[i].z;
00088       color[0] = color_cloud->points[i].r;
00089       color[1] = color_cloud->points[i].g;
00090       color[2] = color_cloud->points[i].b;
00091     }
00092     else if(intensity_cloud)
00093     {
00094       point[0] = intensity_cloud->points[i].x;
00095       point[1] = intensity_cloud->points[i].y;
00096       point[2] = intensity_cloud->points[i].z;
00097       color[0] = intensity_cloud->points[i].intensity * 255.0;
00098       color[1] = intensity_cloud->points[i].intensity * 255.0;
00099       color[2] = intensity_cloud->points[i].intensity * 255.0;
00100     }
00101     else
00102     {
00103       point[0] = xyz_cloud->points[i].x;
00104       point[1] = xyz_cloud->points[i].y;
00105       point[2] = xyz_cloud->points[i].z;
00106       color[0] = 1.0;
00107       color[1] = 0.0;
00108       color[2] = 0.0;
00109     }
00110 
00111     addPoint(tree, point, color);
00112     ++total_points;
00113   }
00114 
00115   printf("%s\n", tree.toString().c_str());
00116   tree.resetCount();
00117 }
00118 
00119 
00120 struct arguments_t {
00121   int cache_size;
00122   char* tree;
00123   unsigned int skip;
00124   unsigned long max_intensity;
00125   std::vector<std::string> las_filenames;
00126 };
00127 
00128 
00129 static int parse_opt(int key, char *arg, struct argp_state *state)
00130 {
00131   struct arguments_t *arguments = (arguments_t*)state->input;
00132   
00133   switch (key)
00134   {
00135   case 'c':
00136     //arguments->cache_size = atoi(arg);
00137     arguments->cache_size = parseNumberSuffixed(arg);
00138     break;
00139   case 't':
00140     arguments->tree = arg;
00141     break;
00142   case 's':
00143     arguments->skip = (unsigned int)parseNumberSuffixed(arg);
00144     break;
00145   case 'i':
00146     arguments->max_intensity = atol(arg);
00147     break;
00148   case ARGP_KEY_ARG:
00149     arguments->las_filenames.push_back(arg);
00150     break;
00151   }
00152   return 0;
00153 }
00154 
00155 
00156 int main (int argc, char** argv)
00157 {
00158   // Default command line arguments
00159   struct arguments_t arguments;
00160   arguments.cache_size = 3000000;
00161   arguments.max_intensity = 255;  // 16384
00162   arguments.skip = 0;
00163   arguments.tree = 0;
00164   
00165   // Parses command line options
00166   struct argp_option options[] = {
00167     {"cache-size", 'c', "SIZE",   0,     "Cache size"},
00168     {"tree",       't', "TREE",   0,     "Path to tree"},
00169     {"skip",       's', "SKIP",   0,     "Number of points to skip"},
00170     {"max-intensity",  'i', "INTENSITY",  0,     "Maximum intensity value"},
00171     { 0 }
00172   };
00173   struct argp argp = { options, parse_opt };
00174   int parse_ok = argp_parse(&argp, argc, argv, 0, 0, &arguments);
00175   printf("Arguments parsed: %d\n", parse_ok);
00176   printf("Cache size: %d\n", arguments.cache_size);
00177   printf("Tree: %s\n", arguments.tree);
00178   if (arguments.skip > 0)
00179     printf("Skipping %u points\n", arguments.skip);
00180   assert(parse_ok == 0);
00181 
00182   if (!arguments.tree) {
00183     fprintf(stderr, "No tree path given.\n");
00184     return 1;
00185   }
00186 
00187   boost::shared_ptr<Storage> storage(openStorage(arguments.tree));
00188   MegaTree tree(storage, arguments.cache_size, false);
00189 
00190   overall_timer.tic();
00191   
00192   Tictoc timer;
00193   for (size_t i = 0; i < arguments.las_filenames.size(); ++i)
00194   {
00195     boost::filesystem::path las_path(arguments.las_filenames[i]);
00196     printf("Importing %s into tree\n", las_path.string().c_str());
00197 
00198     Tictoc one_file_timer;
00199     importPCD(tree, las_path, arguments.max_intensity, &arguments.skip);
00200     float t = one_file_timer.toc();
00201     printf("Flushing %s...\n", las_path.string().c_str());
00202     tree.flushCache();
00203     printf("Finished %s in %.3lf seconds (%.1lf min or %.1lf hours)\n",
00204            las_path.string().c_str(), t, t/60.0f, t/3600.0f);
00205   }
00206 
00207   printf("Flushing the cache\n");
00208   tree.flushCache();
00209   
00210   float t = timer.toc();
00211   unsigned long num_points = tree.getNumPoints();
00212   printf("Importing all files took %.3f seconds for %lu points ==> %.3f sec/million\n", t, num_points, t / num_points * 1e6);
00213 
00214   printf("Finished.\n");
00215   return 0;
00216 }


megatree_pcl
Author(s): Wim Meeussen
autogenerated on Thu Nov 28 2013 11:31:03