import_pcd.cpp

Go to the documentation of this file.

#include <unistd.h>
#include <iostream>
#include <argp.h>

#include <pcl/io/pcd_io.h>
#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>

#include <boost/algorithm/string.hpp>

#include <megatree/common.h>
#include <megatree/megatree.h>
#include <megatree/storage_factory.h>
#include <megatree/tree_functions.h>

#include <fstream>
#include <iostream>

using namespace megatree;

static unsigned long total_points = 0;
static Tictoc overall_timer;

template <class T>
boost::shared_ptr<pcl::PointCloud<T> > removeNan(boost::shared_ptr<pcl::PointCloud<T> > in)
{
  boost::shared_ptr<pcl::PointCloud<T> > out(new pcl::PointCloud<T>);
  pcl::PassThrough<T> pass_filter; 
  pass_filter.setInputCloud(in);
  pass_filter.setFilterFieldName("z");
  pass_filter.setFilterLimits(1e-5, 9999999);
  pass_filter.filter(*out);
  return out;
}

void importPCD(MegaTree &tree, const boost::filesystem::path &path, unsigned long max_intensity, unsigned int *skip = NULL)
{
  //Load the point cloud in blob form so we can check fields
  sensor_msgs::PointCloud2 cloud_blob;
  pcl::io::loadPCDFile(path.string(), cloud_blob);

  //we need to check what fields are available
  int rgb_index = pcl::getFieldIndex(cloud_blob, "rgb");
  int intensity_index = pcl::getFieldIndex(cloud_blob, "intensity");

  //Because of the way PCL does types, this was the easiest thing I could think of to handle
  //different kinds of clouds. Probably not the best, but it'll work
  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > xyz_cloud;
  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > color_cloud;
  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > intensity_cloud;

  //load the cloud in color if the rgb field is available
  if(rgb_index != -1)
  {
    color_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromROSMsg(cloud_blob, *color_cloud);
    color_cloud = removeNan<pcl::PointXYZRGB>(color_cloud);

  }
  //otherwise, if intensity is available load the cloud that way
  else if(intensity_index != 1)
  {
    intensity_cloud.reset(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(cloud_blob, *intensity_cloud);
    intensity_cloud = removeNan<pcl::PointXYZI>(intensity_cloud);
  }
  else
  {
    xyz_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(cloud_blob, *xyz_cloud);
    xyz_cloud = removeNan<pcl::PointXYZ>(xyz_cloud);
  }

  // Adds each point into the tree
  std::vector<double> point(3, 0.0);
  std::vector<double> color(3, 0.0);
  for (unsigned int i = 0; i < cloud_blob.width * cloud_blob.height; i++)
  {
    if (i % 100000 == 0)
      printf("Adding point %u\n", i);

    if(color_cloud)
    {
      point[0] = color_cloud->points[i].x;
      point[1] = color_cloud->points[i].y;
      point[2] = color_cloud->points[i].z;
      color[0] = color_cloud->points[i].r;
      color[1] = color_cloud->points[i].g;
      color[2] = color_cloud->points[i].b;
    }
    else if(intensity_cloud)
    {
      point[0] = intensity_cloud->points[i].x;
      point[1] = intensity_cloud->points[i].y;
      point[2] = intensity_cloud->points[i].z;
      color[0] = intensity_cloud->points[i].intensity * 255.0;
      color[1] = intensity_cloud->points[i].intensity * 255.0;
      color[2] = intensity_cloud->points[i].intensity * 255.0;
    }
    else
    {
      point[0] = xyz_cloud->points[i].x;
      point[1] = xyz_cloud->points[i].y;
      point[2] = xyz_cloud->points[i].z;
      color[0] = 1.0;
      color[1] = 0.0;
      color[2] = 0.0;
    }

    addPoint(tree, point, color);
    ++total_points;
  }

  printf("%s\n", tree.toString().c_str());
  tree.resetCount();
}


struct arguments_t {
  int cache_size;
  char* tree;
  unsigned int skip;
  unsigned long max_intensity;
  std::vector<std::string> las_filenames;
};


static int parse_opt(int key, char *arg, struct argp_state *state)
{
  struct arguments_t *arguments = (arguments_t*)state->input;
  
  switch (key)
  {
  case 'c':
    //arguments->cache_size = atoi(arg);
    arguments->cache_size = parseNumberSuffixed(arg);
    break;
  case 't':
    arguments->tree = arg;
    break;
  case 's':
    arguments->skip = (unsigned int)parseNumberSuffixed(arg);
    break;
  case 'i':
    arguments->max_intensity = atol(arg);
    break;
  case ARGP_KEY_ARG:
    arguments->las_filenames.push_back(arg);
    break;
  }
  return 0;
}


int main (int argc, char** argv)
{
  // Default command line arguments
  struct arguments_t arguments;
  arguments.cache_size = 3000000;
  arguments.max_intensity = 255;  // 16384
  arguments.skip = 0;
  arguments.tree = 0;
  
  // Parses command line options
  struct argp_option options[] = {
    {"cache-size", 'c', "SIZE",   0,     "Cache size"},
    {"tree",       't', "TREE",   0,     "Path to tree"},
    {"skip",       's', "SKIP",   0,     "Number of points to skip"},
    {"max-intensity",  'i', "INTENSITY",  0,     "Maximum intensity value"},
    { 0 }
  };
  struct argp argp = { options, parse_opt };
  int parse_ok = argp_parse(&argp, argc, argv, 0, 0, &arguments);
  printf("Arguments parsed: %d\n", parse_ok);
  printf("Cache size: %d\n", arguments.cache_size);
  printf("Tree: %s\n", arguments.tree);
  if (arguments.skip > 0)
    printf("Skipping %u points\n", arguments.skip);
  assert(parse_ok == 0);

  if (!arguments.tree) {
    fprintf(stderr, "No tree path given.\n");
    return 1;
  }

  boost::shared_ptr<Storage> storage(openStorage(arguments.tree));
  MegaTree tree(storage, arguments.cache_size, false);

  overall_timer.tic();
  
  Tictoc timer;
  for (size_t i = 0; i < arguments.las_filenames.size(); ++i)
  {
    boost::filesystem::path las_path(arguments.las_filenames[i]);
    printf("Importing %s into tree\n", las_path.string().c_str());

    Tictoc one_file_timer;
    importPCD(tree, las_path, arguments.max_intensity, &arguments.skip);
    float t = one_file_timer.toc();
    printf("Flushing %s...\n", las_path.string().c_str());
    tree.flushCache();
    printf("Finished %s in %.3lf seconds (%.1lf min or %.1lf hours)\n",
           las_path.string().c_str(), t, t/60.0f, t/3600.0f);
  }

  printf("Flushing the cache\n");
  tree.flushCache();
  
  float t = timer.toc();
  unsigned long num_points = tree.getNumPoints();
  printf("Importing all files took %.3f seconds for %lu points ==> %.3f sec/million\n", t, num_points, t / num_points * 1e6);

  printf("Finished.\n");
  return 0;
}