/*! 
 * @brief Reads a point cloud and extracts color and depth values as .ppm image 
 */
int main(int argc, char** argv)
{
  readOptions(argc, argv);
  PointCloud<PointXYZRGB>::Ptr p(new PointCloud<PointXYZRGB>());

  PCDReader r;
  if (r.read(file_i, *p) == -1) return(0);
  
  cout << "Loaded pointcloud with " << p->width << " x " << p->height << " points." << endl;

  cob_3d_mapping_common::PPMWriter w;
  if (w.writeRGB(file_o[0], *p) == -1) return(0);
  cout << "Extracted RGB image..." << endl;

  if (file_o[1] != "")
  {
    if (min_z != FLT_MAX)
      w.setMinZ(min_z);
    if (max_z != FLT_MIN)
      w.setMaxZ(max_z);

    if (w.writeDepth(file_o[1], *p) == -1) return(0);
    if (w.writeDepthLinear(file_o[1] + "_linear", *p) == -1) return(0);
    cout << "Extracted depth image..." << endl;
  }

  cout << "Done!" << endl;

  return(0);
}
int
main (int argc, char **argv)
{
  if (argc != 2)
  {
    PCL_ERROR ("Syntax: ./multiscale_feature_persistence_example [path_to_cloud.pcl]\n");
    return -1;
  }

  PointCloud<PointXYZ>::Ptr cloud_scene (new PointCloud<PointXYZ> ());
  PCDReader reader;
  reader.read (argv[1], *cloud_scene);

  PointCloud<PointXYZ>::Ptr cloud_subsampled;
  PointCloud<Normal>::Ptr cloud_subsampled_normals;
  subsampleAndCalculateNormals (cloud_scene, cloud_subsampled, cloud_subsampled_normals);

  PCL_INFO ("STATS:\ninitial point cloud size: %u\nsubsampled point cloud size: %u\n", cloud_scene->points.size (), cloud_subsampled->points.size ());
  visualization::CloudViewer viewer ("Multiscale Feature Persistence Example Visualization");
  viewer.showCloud (cloud_scene, "scene");


  MultiscaleFeaturePersistence<PointXYZ, FPFHSignature33> feature_persistence;
  std::vector<float> scale_values;
  for (float x = 2.0f; x < 3.6f; x += 0.35f)
    scale_values.push_back (x / 100.0f);
  feature_persistence.setScalesVector (scale_values);
  feature_persistence.setAlpha (1.3f);
  FPFHEstimation<PointXYZ, Normal, FPFHSignature33>::Ptr fpfh_estimation (new FPFHEstimation<PointXYZ, Normal, FPFHSignature33> ());
  fpfh_estimation->setInputCloud (cloud_subsampled);
  fpfh_estimation->setInputNormals (cloud_subsampled_normals);
  pcl::search::KdTree<PointXYZ>::Ptr tree (new pcl::search::KdTree<PointXYZ> ());
  fpfh_estimation->setSearchMethod (tree);
  feature_persistence.setFeatureEstimator (fpfh_estimation);
  feature_persistence.setDistanceMetric (pcl::CS);

  PointCloud<FPFHSignature33>::Ptr output_features (new PointCloud<FPFHSignature33> ());
  boost::shared_ptr<std::vector<int> > output_indices (new std::vector<int> ());
  feature_persistence.determinePersistentFeatures (*output_features, output_indices);

  PCL_INFO ("persistent features cloud size: %u\n", output_features->points.size ());

  ExtractIndices<PointXYZ> extract_indices_filter;
  extract_indices_filter.setInputCloud (cloud_subsampled);
  extract_indices_filter.setIndices (output_indices);
  PointCloud<PointXYZ>::Ptr persistent_features_locations (new PointCloud<PointXYZ> ());
  extract_indices_filter.filter (*persistent_features_locations);

  viewer.showCloud (persistent_features_locations, "persistent features");
  PCL_INFO ("Persistent features have been computed. Waiting for the user to quit the visualization window.\n");

//  io::savePCDFileASCII ("persistent_features.pcd", *persistent_features_locations);
//  PCL_INFO ("\nPersistent feature locations written to persistent_features.pcd\n");

  while (!viewer.wasStopped (50));

  return 0;
}
Beispiel #3
0
void 
pcl::PCDGrabberBase::PCDGrabberImpl::readAhead ()
{
  PCDReader reader;
  int pcd_version;

  // Check if we're still reading files from a TAR file
  if (tar_fd_ != -1)
  {
    if (!readTARHeader ())
      return;
    valid_ = (reader.read (tar_file_, next_cloud_, origin_, orientation_, pcd_version, tar_offset_) == 0);
    if (!valid_)
      closeTARFile ();
    else
    {
      tar_offset_ += (tar_header_.getFileSize ()) + (512 - tar_header_.getFileSize () % 512);
      int result = static_cast<int> (pcl_lseek (tar_fd_, tar_offset_, SEEK_SET));
      if (result < 0)
        closeTARFile ();
    }
  }
  // We're not still reading from a TAR file, so check if there are other PCD/TAR files in the list
  else
  {
    if (pcd_iterator_ != pcd_files_.end ())
    {
      // Try to read in the file as a PCD first
      valid_ = (reader.read (*pcd_iterator_, next_cloud_, origin_, orientation_, pcd_version) == 0);

      // Has an error occured? Check if we can interpret the file as a TAR file first before going onto the next
      if (!valid_ && openTARFile (*pcd_iterator_) >= 0 && readTARHeader ())
      {
        tar_file_ = *pcd_iterator_;
        valid_ = (reader.read (tar_file_, next_cloud_, origin_, orientation_, pcd_version, tar_offset_) == 0);
        if (!valid_)
          closeTARFile ();
        else
        {
          tar_offset_ += (tar_header_.getFileSize ()) + (512 - tar_header_.getFileSize () % 512);
          int result = static_cast<int> (pcl_lseek (tar_fd_, tar_offset_, SEEK_SET));
          if (result < 0)
            closeTARFile ();
        }
      }

      if (++pcd_iterator_ == pcd_files_.end () && repeat_)
        pcd_iterator_ = pcd_files_.begin ();
    }
    else
      valid_ = false;
  }
}
Beispiel #4
0
            DataSource(const std::string& file)
                : cloud(new PointCloud<PointXYZ>()), surface(new PointCloud<PointXYZ>()), indices( new std::vector<int>() ),
                normals(new PointCloud<Normal>()), normals_surface(new PointCloud<Normal>())
            {
                PCDReader pcd;
                pcd.read(file, *cloud);

                PointXYZ minp, maxp;
                pcl::getMinMax3D(*cloud, minp, maxp);
                float sz = (maxp.x - minp.x + maxp.y - minp.y + maxp.z - minp.z) / 3;
                radius = sz / 15;
            }
int
main (int, char **argv)
{
  PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());

  PCDReader reader;
  reader.read (argv[1], *cloud);
  PCL_INFO ("Cloud read: %s\n", argv[1]);
  cerr << "cloud has #points: " << cloud->points.size () << endl;

  PointCloud<PointXYZ>::Ptr cloud_subsampled (new PointCloud<PointXYZ> ());
  VoxelGrid<PointXYZ> subsampling_filter;
  subsampling_filter.setInputCloud (cloud);
  subsampling_filter.setLeafSize (subsampling_leaf_size, subsampling_leaf_size, subsampling_leaf_size);
  subsampling_filter.filter (*cloud_subsampled);
  cerr << "subsampled cloud has #points: " << cloud_subsampled->points.size () << endl;

  StatisticalMultiscaleInterestRegionExtraction<PointXYZ> region_extraction;
  std::vector<float> scale_vector;
  PCL_INFO ("Scale values that will be used: ");
  float base_scale_aux = base_scale;
  for (size_t scales = 0; scales < 7; ++scales)
  {
    PCL_INFO ("%f ", base_scale_aux);
    scale_vector.push_back (base_scale_aux);
    base_scale_aux *= 1.6f;
  }
  PCL_INFO ("\n");
  region_extraction.setInputCloud (cloud_subsampled);
  region_extraction.setScalesVector (scale_vector);
  std::list<IndicesPtr> rois;
  region_extraction.computeRegionsOfInterest (rois);

  PCL_INFO ("Regions of interest found: %d\n", rois.size ());
  pcl::ExtractIndices<PointXYZ> extract_indices_filter;
  unsigned int roi_count = 0;
  for (std::list<IndicesPtr>::iterator l_it = rois.begin (); l_it != rois.end (); ++l_it)
  {
    PointCloud<PointXYZ> roi_points;
    extract_indices_filter.setInputCloud (cloud_subsampled);
    extract_indices_filter.setIndices (*l_it);
    extract_indices_filter.filter (roi_points);

    char filename[512];
    sprintf (filename, "roi_%03d.pcd", ++roi_count);
    io::savePCDFileASCII (filename, roi_points);
  }

  io::savePCDFileASCII ("subsampled_input.pcd", *cloud_subsampled);

  return 0;
}
/* ---[ */
int
main (int argc, char** argv)
{
  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices;
  p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_file_indices.empty ())
  {
    print_error ("  Need at least an input PCD file (e.g. scene.pcd) to continue!\n\n");
    print_info ("Ideally, need an input file, and three output PCD files, e.g., object.pcd, plane.pcd, rest.pcd\n");
    return (-1);
  }
  
  string object_file = "object.pcd", plane_file = "plane.pcd", rest_file = "rest.pcd";
  if (p_file_indices.size () >= 4)
    rest_file = argv[p_file_indices[3]];
  if (p_file_indices.size () >= 3)
    plane_file = argv[p_file_indices[2]];
  if (p_file_indices.size () >= 2)
    object_file = argv[p_file_indices[1]];


  PCDReader reader;
  // Test the header
  pcl::PCLPointCloud2 dummy;
  reader.readHeader (argv[p_file_indices[0]], dummy);
  if (dummy.height != 1 && getFieldIndex (dummy, "rgba") != -1)
  {
    print_highlight ("Enabling 2D image viewer mode.\n");
    ObjectSelection<PointXYZRGBA> s;
    if (!s.load (argv[p_file_indices[0]])) return (-1);
    s.initGUI ();
    s.compute ();
    s.save (object_file, plane_file);
  }
  else
  {
    ObjectSelection<PointXYZ> s;
    if (!s.load (argv[p_file_indices[0]])) return (-1);
    s.initGUI ();
    s.compute ();
    s.save (object_file, plane_file);
  }

  return (0);
}
Beispiel #7
0
    // TODO Enum for is_test_phase
    PointCloud<Signature>::Ptr
    Detector::obtainFeatures(Scene &scene, PointCloud<PointNormal>::Ptr keypoints, bool is_test_phase, bool cache)
    {
      if (cache == false)
      {
        PointCloud<Signature>::Ptr features = computeFeatures(scene.cloud, keypoints);
        return features;
      }
      else
      {
        std::string name_str = std::string(feature_est_->name_) + scene.id;

        if (is_test_phase) {
          name_str += "_test";
        }
        else {
          name_str += "_train";
        }

        name_str += ".pcd";

        const char *name = name_str.c_str();

        if (ifstream(name)) {
          PointCloud<Signature>::Ptr features (new PointCloud<Signature>());
          PCDReader r;
          r.readEigen(std::string(name), *features);
          //*features = *tmp;
          //io::loadPCDFile(name, *features);
          if (features->size() != keypoints->size())
            assert(false);
          cout << "got " << features->size() << " features from " << keypoints->size() << " points" << endl;
          return features;
        } else {
          PointCloud<Signature>::Ptr features = computeFeatures(scene.cloud, keypoints);
          PCDWriter w;
          w.writeASCIIEigen(std::string(name), *features);
          return features;
        }
      }
    }
Beispiel #8
0
int
main (int argc, char **argv)
{
  if (argc != 5)
  {
    PCL_ERROR ("./surfel_smoothing_test normal_search_radius surfel_scale source_cloud destination_cloud\n");
    return (-1);
  }
  PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
  PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());

  PCDReader reader;
  reader.read (argv[3], *cloud);
  PCL_INFO ("Cloud read: %s\n", argv[3]);

  double normal_search_radius = static_cast<double> (atof (argv[1]));
  double surfel_scale = static_cast<double> (atof (argv[2]));


  NormalEstimation<PointXYZ, Normal> normal_estimation;
  normal_estimation.setInputCloud (cloud);
  search::KdTree<PointXYZ>::Ptr search_tree (new search::KdTree<PointXYZ>);
  normal_estimation.setSearchMethod (search_tree);
  normal_estimation.setRadiusSearch (normal_search_radius);
  normal_estimation.compute (*normals);

  SurfelSmoothing<PointXYZ, Normal> surfel_smoothing (surfel_scale);
  surfel_smoothing.setInputCloud (cloud);
  surfel_smoothing.setInputNormals (normals);
  surfel_smoothing.setSearchMethod (search_tree);
  PointCloud<PointXYZ>::Ptr output_positions;
  PointCloud<Normal>::Ptr output_normals;
  surfel_smoothing.computeSmoothedCloud (output_positions, output_normals);

  PointCloud<PointNormal>::Ptr output_with_normals (new PointCloud<PointNormal> ());
  pcl::concatenateFields (*output_positions, *normals, *output_with_normals);

  io::savePCDFileASCII (argv[4], *output_with_normals);

  return (0);
}
Beispiel #9
0
int
main (int argc, char** argv)
{
  if (argc != 3)
  {
    PCL_ERROR ("Syntax: ./ppf_object_recognition pcd_model_list pcd_scene\n");
    return -1;
  }


  /// read point clouds from HDD
  PCL_INFO ("Reading scene ...\n");
  PointCloud<PointXYZ>::Ptr cloud_scene (new PointCloud<PointXYZ> ());
  PCDReader reader;
  reader.read (argv[2], *cloud_scene);
  PCL_INFO ("Scene read: %s\n", argv[2]);

  PCL_INFO ("Reading models ...\n");
  vector<PointCloud<PointXYZ>::Ptr > cloud_models;
  ifstream pcd_file_list (argv[1]);
  while (!pcd_file_list.eof())
  {
    char str[512];
    pcd_file_list.getline (str, 512);
    PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
    reader.read (str, *cloud);
    cloud_models.push_back (cloud);
    PCL_INFO ("Model read: %s\n", str);
  }

  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (1000);
  seg.setDistanceThreshold (0.05);
  extract.setNegative (true);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
  unsigned nr_points = unsigned (cloud_scene->points.size ());
  while (cloud_scene->points.size () > 0.3 * nr_points)
  {
    seg.setInputCloud (cloud_scene);
    seg.segment (*inliers, *coefficients);
    PCL_INFO ("Plane inliers: %u\n", inliers->indices.size ());
    if (inliers->indices.size () < 50000) break;

    extract.setInputCloud (cloud_scene);
    extract.setIndices (inliers);
    extract.filter (*cloud_scene);
  }

  PointCloud<PointNormal>::Ptr cloud_scene_input = subsampleAndCalculateNormals (cloud_scene);
  vector<PointCloud<PointNormal>::Ptr > cloud_models_with_normals;

  PCL_INFO ("Training models ...\n");
  vector<PPFHashMapSearch::Ptr> hashmap_search_vector;
  for (size_t model_i = 0; model_i < cloud_models.size (); ++model_i)
  {
    PointCloud<PointNormal>::Ptr cloud_model_input = subsampleAndCalculateNormals (cloud_models[model_i]);
    cloud_models_with_normals.push_back (cloud_model_input);

    PointCloud<PPFSignature>::Ptr cloud_model_ppf (new PointCloud<PPFSignature> ());
    PPFEstimation<PointNormal, PointNormal, PPFSignature> ppf_estimator;
    ppf_estimator.setInputCloud (cloud_model_input);
    ppf_estimator.setInputNormals (cloud_model_input);
    ppf_estimator.compute (*cloud_model_ppf);

    PPFHashMapSearch::Ptr hashmap_search (new PPFHashMapSearch (12.0f / 180.0f * float (M_PI),
                                                                 0.05f));
    hashmap_search->setInputFeatureCloud (cloud_model_ppf);
    hashmap_search_vector.push_back (hashmap_search);
  }


  visualization::PCLVisualizer viewer ("PPF Object Recognition - Results");
  viewer.setBackgroundColor (0, 0, 0);
  viewer.addPointCloud (cloud_scene);
  viewer.spinOnce (10);
  PCL_INFO ("Registering models to scene ...\n");
  for (size_t model_i = 0; model_i < cloud_models.size (); ++model_i)
  {

    PPFRegistration<PointNormal, PointNormal> ppf_registration;
    // set parameters for the PPF registration procedure
    ppf_registration.setSceneReferencePointSamplingRate (10);
    ppf_registration.setPositionClusteringThreshold (0.2f);
    ppf_registration.setRotationClusteringThreshold (30.0f / 180.0f * float (M_PI));
    ppf_registration.setSearchMethod (hashmap_search_vector[model_i]);
    ppf_registration.setInputCloud (cloud_models_with_normals[model_i]);
    ppf_registration.setInputTarget (cloud_scene_input);

    PointCloud<PointNormal> cloud_output_subsampled;
    ppf_registration.align (cloud_output_subsampled);

    PointCloud<PointXYZ>::Ptr cloud_output_subsampled_xyz (new PointCloud<PointXYZ> ());
    for (size_t i = 0; i < cloud_output_subsampled.points.size (); ++i)
      cloud_output_subsampled_xyz->points.push_back ( PointXYZ (cloud_output_subsampled.points[i].x, cloud_output_subsampled.points[i].y, cloud_output_subsampled.points[i].z));


    Eigen::Matrix4f mat = ppf_registration.getFinalTransformation ();
    Eigen::Affine3f final_transformation (mat);


    //  io::savePCDFileASCII ("output_subsampled_registered.pcd", cloud_output_subsampled);

    PointCloud<PointXYZ>::Ptr cloud_output (new PointCloud<PointXYZ> ());
    pcl::transformPointCloud (*cloud_models[model_i], *cloud_output, final_transformation);


    stringstream ss; ss << "model_" << model_i;
    visualization::PointCloudColorHandlerRandom<PointXYZ> random_color (cloud_output->makeShared ());
    viewer.addPointCloud (cloud_output, random_color, ss.str ());
//    io::savePCDFileASCII (ss.str ().c_str (), *cloud_output);
    PCL_INFO ("Showing model %s\n", ss.str ().c_str ());
  }

  PCL_INFO ("All models have been registered!\n");


  while (!viewer.wasStopped ())
  {
    viewer.spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  return 0;
}
int main(int argc, char** argv)
{
  readOptions(argc, argv);
  boost::timer t;

  // 3D point clouds
  PointCloud<PointXYZRGB>::Ptr p(new PointCloud<PointXYZRGB>);
  PointCloud<Normal>::Ptr n(new PointCloud<Normal>);
  PointCloud<PrincipalCurvatures>::Ptr pc(new PointCloud<PrincipalCurvatures>);
  PointCloud<PointLabel>::Ptr l(new PointCloud<PointLabel>);

  PCDReader r;
  if (r.read(file_in_, *p) == -1) return(0);

  vector<PointIndices> indices;
  ifstream fs;

  string line;
  fs.open(file_cluster_.c_str());
  if (fs.is_open())
  {
    while(fs.good())
    {
      getline (fs,line);
      istringstream iss(line);
      PointIndices pi;
      do
      {
        int temp;
        iss >> temp;
        pi.indices.push_back(temp);
      } while(iss);
      if(pi.indices.size()>0)
        indices.push_back(pi);
    }
  }
  std::cout << "indices file read successfully" << std::endl;

  // --- Normal Estimation ---
  if (en_one_)
  {
    t.restart();
    cob_3d_features::OrganizedNormalEstimation<PointXYZRGB, Normal, PointLabel>one;
    one.setInputCloud(p);
    one.setOutputLabels(l);
    //one.setPixelSearchRadius(pns_n_,points_,circle_); //radius,pixel,circle
    one.setPixelSearchRadius(8,2,2);
    one.setSkipDistantPointThreshold(12);
    one.compute(*n);
    cout << t.elapsed() << "s\t for Organized Normal Estimation" << endl;
  }
  else
  {
    t.restart();
    #ifdef PCL_VERSION_COMPARE //fuerte
      pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
    #else //electric
      pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
    #endif
    NormalEstimation<PointXYZRGB, Normal> ne;
    ne.setRadiusSearch(rn_);
    ne.setSearchMethod(tree);
    ne.setInputCloud(p);
    ne.compute(*n);
    cout << t.elapsed() << "s\t for normal estimation" << endl;
  }
  cob_3d_features::OrganizedCurvatureEstimationOMP<PointXYZRGB, Normal, PointLabel, PrincipalCurvatures> oce;
  oce.setInputCloud(p);
  oce.setInputNormals(n);
  oce.setPixelSearchRadius(8,2,2);
  oce.setSkipDistantPointThreshold(12);

  //KdTreeFLANN<PointXYZRGB>::Ptr tree(new KdTreeFLANN<PointXYZRGB>);
  //ne.setRadiusSearch(rn_);
  //ne.setSearchMethod(tree);
  if (indices.size() == 0)
  {
    oce.setOutputLabels(l);
    oce.compute(*pc);
    cob_3d_features::CurvatureClassifier<PrincipalCurvatures, PointLabel>cc;
    cc.setInputCloud(pc);
    cc.classify(*l);
  }
  else
  {
    for(unsigned int i=0; i<indices.size(); i++)
    {
      std::cout << "cluster " << i << " has " << indices[i].indices.size() << " points" << std::endl;
      if(i==3)
      {
        /*for(unsigned int j=0; j<indices[i].indices.size(); j++)
          std::cout << indices[i].indices[j] << ",";
          std::cout << std::endl;*/
        //cout << i << ": " << indices[i].indices.front() << "!" << endl;
        t.restart();
        boost::shared_ptr<PointIndices> ind_ptr = boost::make_shared<PointIndices>(indices[i]);
        std::cout << ind_ptr->indices.size() << std::endl;
        oce.setIndices(ind_ptr);
        oce.setOutputLabels(l);
        oce.compute(*pc);
        cout << t.elapsed() << "s\t for principal curvature estimation" << endl;

        cob_3d_features::CurvatureClassifier<PrincipalCurvatures, PointLabel>cc;
        cc.setInputCloud(pc);
        cc.setIndices(ind_ptr);
        cc.classify(*l);
      }
    }
  }
  // colorize edges of 3d point cloud
  for (size_t i = 0; i < l->points.size(); i++)
  {
    //std::cout << l->points[i].label << std::endl;
    if (l->points[i].label == I_UNDEF)
    {
      p->points[i].r = 0;
      p->points[i].g = 0;
      p->points[i].b = 0;
    }
    else if (l->points[i].label == I_NAN)
    {
      p->points[i].r = 0;
      p->points[i].g = 255;
      p->points[i].b = 0;
    }
    else if (l->points[i].label == I_EDGE)
    {
      p->points[i].r = 0;
      p->points[i].g = 0;
      p->points[i].b = 255;
    }
    else if (l->points[i].label == I_BORDER)
    {
      p->points[i].r = 255;
      p->points[i].g = 0;
      p->points[i].b = 0;
    }
    else if (l->points[i].label == I_PLANE)
    {
      p->points[i].r = 255;
      p->points[i].g = 255;
      p->points[i].b = 0;
    }
    else if (l->points[i].label == I_CYL)
    {
      p->points[i].r = 255;
      p->points[i].g = 0;
      p->points[i].b = 255;
    }
    else
    {
      p->points[i].r = 255;
      p->points[i].g = 255;
      p->points[i].b = 255;
    }
  }

  visualization::PCLVisualizer v;
  v.setBackgroundColor(0,127,127);
  ColorHdlRGB col_hdl(p);
  v.addPointCloud<PointXYZRGB>(p,col_hdl, "segmented");

  while(!v.wasStopped())
  {
    v.spinOnce(100);
    usleep(100000);
  }


  return(0);
}
int
main (int argc, char **argv)
{
  if (argc != 3)
  {
    PCL_ERROR ("Syntax: ./pyramid_surface_matching model_1 model_2\n");
    return -1;
  }


  /// read point clouds from HDD
  PCL_INFO ("Reading scene ...\n");
  PointCloud<PointXYZ>::Ptr cloud_a (new PointCloud<PointXYZ> ()),
      cloud_b (new PointCloud<PointXYZ> ());
  PCDReader reader;
  reader.read (argv[1], *cloud_a);
  reader.read (argv[2], *cloud_b);

  PointCloud<PointXYZ>::Ptr cloud_a_subsampled, cloud_b_subsampled;
  PointCloud<Normal>::Ptr cloud_a_subsampled_normals, cloud_b_subsampled_normals;
  subsampleAndCalculateNormals (cloud_a, cloud_a_subsampled, cloud_a_subsampled_normals);
  subsampleAndCalculateNormals (cloud_b, cloud_b_subsampled, cloud_b_subsampled_normals);

  PCL_INFO ("Finished subsampling the clouds ...\n");


  PointCloud<PPFSignature>::Ptr ppf_signature_a (new PointCloud<PPFSignature> ()),
      ppf_signature_b (new PointCloud<PPFSignature> ());
  PointCloud<PointNormal>::Ptr cloud_subsampled_with_normals_a (new PointCloud<PointNormal> ()),
      cloud_subsampled_with_normals_b (new PointCloud<PointNormal> ());
  concatenateFields (*cloud_a_subsampled, *cloud_a_subsampled_normals, *cloud_subsampled_with_normals_a);
  concatenateFields (*cloud_b_subsampled, *cloud_b_subsampled_normals, *cloud_subsampled_with_normals_b);

  PPFEstimation<PointNormal, PointNormal, PPFSignature> ppf_estimator;
  ppf_estimator.setInputCloud (cloud_subsampled_with_normals_a);
  ppf_estimator.setInputNormals (cloud_subsampled_with_normals_a);
  ppf_estimator.compute (*ppf_signature_a);
  ppf_estimator.setInputCloud (cloud_subsampled_with_normals_b);
  ppf_estimator.setInputNormals (cloud_subsampled_with_normals_b);
  ppf_estimator.compute (*ppf_signature_b);

  PCL_INFO ("Feature cloud sizes: %u , %u\n", ppf_signature_a->points.size (), ppf_signature_b->points.size ());

  PCL_INFO ("Finished calculating the features ...\n");
  vector<pair<float, float> > dim_range_input, dim_range_target;
  for (size_t i = 0; i < 3; ++i) dim_range_input.push_back (pair<float, float> ((float) -M_PI, (float) M_PI));
  dim_range_input.push_back (pair<float, float> (0.0f, 1.0f));
  for (size_t i = 0; i < 3; ++i) dim_range_target.push_back (pair<float, float> ((float) -M_PI * 10.0f, (float) M_PI * 10.0f));
  dim_range_target.push_back (pair<float, float> (0.0f, 50.0f));


  PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_a (new PyramidFeatureHistogram<PPFSignature> ());
  pyramid_a->setInputCloud (ppf_signature_a);
  pyramid_a->setInputDimensionRange (dim_range_input);
  pyramid_a->setTargetDimensionRange (dim_range_target);
  pyramid_a->compute ();
  PCL_INFO ("Done with the first pyramid\n");

  PyramidFeatureHistogram<PPFSignature>::Ptr pyramid_b (new PyramidFeatureHistogram<PPFSignature> ());
  pyramid_b->setInputCloud (ppf_signature_b);
  pyramid_b->setInputDimensionRange (dim_range_input);
  pyramid_b->setTargetDimensionRange (dim_range_target);
  pyramid_b->compute ();
  PCL_INFO ("Done with the second pyramid\n");

  float value = PyramidFeatureHistogram<PPFSignature>::comparePyramidFeatureHistograms (pyramid_a, pyramid_b);
  PCL_INFO ("Surface comparison value between %s and %s is: %f\n", argv[1], argv[2], value);


  return 0;
}
int main(int argc, char** argv)
{
  readOptions(argc, argv);

  PointCloud<PointXYZRGB>::Ptr p_raw(new PointCloud<PointXYZRGB>());
  PointCloud<PointXYZRGB>::Ptr p_rsd_out(new PointCloud<PointXYZRGB>());
  PointCloud<PointXYZRGB>::Ptr p_rsd_ref(new PointCloud<PointXYZRGB>());
  PointCloud<PointXYZRGB>::Ptr p_pc_out(new PointCloud<PointXYZRGB>());
  PointCloud<PointXYZRGB>::Ptr p_pc_ref(new PointCloud<PointXYZRGB>());
  PointCloud<PointXYZRGB>::Ptr p_fpfh_out(new PointCloud<PointXYZRGB>());
  PointCloud<PointXYZRGB>::Ptr p_fpfh_ref(new PointCloud<PointXYZRGB>());

  PCDReader r;
  if(r.read(file_in_, *p_raw) == -1)
    cout << "Could not read file " << file_in_ << endl;
  cout << "Read pcd file \"" << file_in_ << "\" (Points: " << p_raw->points.size() << ", width: "
       << p_raw->width << ", height: " << p_raw->height << ")" << endl;

  cout << "Headline: \n\n"<< cob_3d_mapping_common::writeHeader() << endl << endl;

  if (rsd_enable_)
    processRSD(p_raw, p_rsd_ref, p_rsd_out);
  if (pc_enable_)
    processPC(p_raw, p_pc_ref, p_pc_out);
  if (fpfh_enable_)
    processFPFH(p_raw, p_fpfh_ref, p_fpfh_out);

  if (quiet_) return (0);

  boost::shared_ptr<visualization::PCLVisualizer> v;
  if (camera_pos_ != "")
  {
    int argc_dummy = 3;
    char *argv_dummy[] = {(char*)argv[0], "-cam", (char*)camera_pos_.c_str(), NULL};
    v.reset(new visualization::PCLVisualizer(argc_dummy, argv_dummy, file_in_));
  }
  else
  {
    v.reset(new visualization::PCLVisualizer(file_in_));
  }



  /* --- Viewports: ---
   *  1y
   *    | 1 | 3 |
   * .5 ----+----
   *    | 2 | 4 |
   *  0    .5    1x
   * 1:
   */
  int v1(0);
  ColorHdlRGB col_hdl1(p_rsd_ref);
  v->createViewPort(0.0, 0.5, 0.5, 1.0, v1);
  v->setBackgroundColor(255,255,255, v1);
  v->addPointCloud<PointXYZRGB>(p_rsd_ref, col_hdl1, "raw", v1);

  // 2:
  int v2(0);
  ColorHdlRGB col_hdl2(p_fpfh_out);
  v->createViewPort(0.0, 0.0, 0.5, 0.5, v2);
  v->setBackgroundColor(255,255,255, v2);
  v->addPointCloud<PointXYZRGB>(p_fpfh_out, col_hdl2, "fpfh", v2);

  // 3:
  int v3(0);
  ColorHdlRGB col_hdl3(p_rsd_out);
  v->createViewPort(0.5, 0.5, 1.0, 1.0, v3);
  v->setBackgroundColor(255,255,255, v3);
  v->addPointCloud<PointXYZRGB>(p_rsd_out, col_hdl3, "rsd", v3);

  // 4:
  int v4(0);
  ColorHdlRGB col_hdl4(p_pc_out);
  v->createViewPort(0.5, 0.0, 1.0, 0.5, v4);
  v->setBackgroundColor(255,255,255, v4);
  v->addPointCloud<PointXYZRGB>(p_pc_out, col_hdl4, "pc", v4);


  while(!v->wasStopped())
  {
    v->spinOnce(100);
    usleep(100000);
  }
  return(0);
}
int
main (int argc, char** argv)
{
  if (argc < 2)
  {
    std::cerr << "Error: Please specify a PCD file (rosrun cob_3d_features profile_ne test.pcd)." << std::endl;
    return -1;
  }
  PointCloud<PointXYZ>::Ptr p (new PointCloud<PointXYZ>);
  PointCloud<Normal>::Ptr n (new PointCloud<Normal>);
  PointCloud<InterestPoint>::Ptr ip (new PointCloud<InterestPoint>);
  pcl::PointCloud<pcl::PointNormal> p_n;
  PrecisionStopWatch t;
  std::string file_ = argv[1];
  PCDReader r;
  if (r.read (file_, *p) == -1)
    return -1;

  Eigen::Vector3f normal;
  determinePlaneNormal (p, normal);
  //std::cout << normal << std::endl;

  cob_3d_features::OrganizedNormalEstimation<PointXYZ, Normal, PointLabel> ne;
  ne.setPixelSearchRadius (8, 2, 2);
  //ne.setSkipDistantPointThreshold(8);
  ne.setInputCloud (p);
  PointCloud<PointLabel>::Ptr labels (new PointCloud<PointLabel>);
  ne.setOutputLabels (labels);
  t.precisionStart ();
  ne.compute (*n);
  std::cout << t.precisionStop () << "s\t for organized normal estimation" << std::endl;

  cob_3d_features::OrganizedNormalEstimationOMP<PointXYZ, Normal, PointLabel> ne_omp;
  ne_omp.setPixelSearchRadius (8, 2, 2);
  //ne.setSkipDistantPointThreshold(8);
  ne_omp.setInputCloud (p);
  //PointCloud<PointLabel>::Ptr labels(new PointCloud<PointLabel>);
  ne_omp.setOutputLabels (labels);
  t.precisionStart ();
  ne_omp.compute (*n);
  std::cout << t.precisionStop () << "s\t for organized normal estimation (OMP)" << std::endl;
  concatenateFields (*p, *n, p_n);
  io::savePCDFileASCII ("normals_organized.pcd", p_n);

  double good_thr = 0.97;
  unsigned int ctr = 0, nan_ctr = 0;
  double d_sum = 0;
  for (unsigned int i = 0; i < p->size (); i++)
  {
    if (pcl_isnan(n->points[i].normal[0]))
    {
      nan_ctr++;
      continue;
    }
    double d = normal.dot (n->points[i].getNormalVector3fMap ());
    d_sum += fabs (1 - fabs (d));
    if (fabs (d) > good_thr)
      ctr++;
  }
  std::cout << "Average error: " << d_sum / p->size () << std::endl;
  std::cout << "Ratio of good normals: " << (double)ctr / p->size () << std::endl;
  std::cout << "Invalid normals: " << nan_ctr << std::endl;

  IntegralImageNormalEstimation<PointXYZ, Normal> ne2;
  ne2.setNormalEstimationMethod (ne2.COVARIANCE_MATRIX);
  ne2.setMaxDepthChangeFactor (0.02f);
  ne2.setNormalSmoothingSize (10.0f);
  ne2.setDepthDependentSmoothing (true);
  ne2.setInputCloud (p);
  t.precisionStart ();
  ne2.compute (*n);
  std::cout << t.precisionStop () << "s\t for integral image normal estimation" << std::endl;
  concatenateFields (*p, *n, p_n);
  io::savePCDFileASCII ("normals_integral.pcd", p_n);

  ctr = 0;
  nan_ctr = 0;
  d_sum = 0;
  for (unsigned int i = 0; i < p->size (); i++)
  {
    if (pcl_isnan(n->points[i].normal[0]))
    {
      nan_ctr++;
      continue;
    }
    double d = normal.dot (n->points[i].getNormalVector3fMap ());
    d_sum += fabs (1 - fabs (d));
    if (fabs (d) > good_thr)
      ctr++;
  }
  std::cout << "Average error: " << d_sum / p->size () << std::endl;
  std::cout << "Ratio of good normals: " << (double)ctr / p->size () << std::endl;
  std::cout << "Invalid normals: " << nan_ctr << std::endl;

  NormalEstimationOMP<PointXYZ, Normal> ne3;
  ne3.setInputCloud (p);
  ne3.setNumberOfThreads (4);
  ne3.setKSearch (256);
  //ne3.setRadiusSearch(0.01);
  t.precisionStart ();
  ne3.compute (*n);
  std::cout << t.precisionStop () << "s\t for vanilla normal estimation" << std::endl;
  concatenateFields (*p, *n, p_n);
  io::savePCDFileASCII ("normals_vanilla.pcd", p_n);

  ctr = 0;
  nan_ctr = 0;
  d_sum = 0;
  for (unsigned int i = 0; i < p->size (); i++)
  {
    if (pcl_isnan(n->points[i].normal[0]))
    {
      nan_ctr++;
      continue;
    }
    double d = normal.dot (n->points[i].getNormalVector3fMap ());
    d_sum += fabs (1 - fabs (d));
    if (fabs (d) > good_thr)
      ctr++;
  }
  std::cout << "Average error: " << d_sum / p->size () << std::endl;
  std::cout << "Ratio of good normals: " << (double)ctr / p->size () << std::endl;
  std::cout << "Invalid normals: " << nan_ctr << std::endl;

  return 0;
}
int main(int argc, char** argv)
{
	if (argc < 2)
	{
		cout << "Input a PCD file name...\n";
		return 0;
	}

	PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>), cloud_f(new PointCloud<PointXYZ>);
	PCDReader reader;
	reader.read(argv[1], *cloud);
	cout << "PointCloud before filtering has: " << cloud->points.size() << " data points.\n";

	PointCloud<PointXYZ>::Ptr cloud_filtered(new PointCloud<PointXYZ>);
	VoxelGrid<PointXYZ> vg;
	vg.setInputCloud(cloud);
	vg.setLeafSize(0.01f, 0.01f, 0.01f);
	vg.filter(*cloud_filtered);
	cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points.\n";

	SACSegmentation<PointXYZ> seg;
	PointIndices::Ptr inliers(new PointIndices);
	PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ>);

	ModelCoefficients::Ptr coefficients(new ModelCoefficients);
	seg.setOptimizeCoefficients(true);
	seg.setModelType(SACMODEL_PLANE);
	seg.setMethodType(SAC_RANSAC);
	seg.setMaxIterations(100);
	seg.setDistanceThreshold(0.02);

	int i=0, nr_points = (int)cloud_filtered->points.size();
	while (cloud_filtered->points.size() > 0.3 * nr_points)
	{
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size() == 0)
		{
			cout << "Coud not estimate a planar model for the given dataset.\n";
			break;
		}

		ExtractIndices<PointXYZ> extract;
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_plane);
		cout << "PointCloud representing the planar component has: " << cloud_filtered->points.size() << " data points.\n";

		extract.setNegative(true);
		extract.filter(*cloud_f);
		cloud_filtered->swap(*cloud_f);
	}

	search::KdTree<PointXYZ>::Ptr kdtree(new search::KdTree<PointXYZ>);
	kdtree->setInputCloud(cloud_filtered);

	vector<PointIndices> cluster_indices;
	EuclideanClusterExtraction<PointXYZ> ece;
	ece.setClusterTolerance(0.02);
	ece.setMinClusterSize(100);
	ece.setMaxClusterSize(25000);
	ece.setSearchMethod(kdtree);
	ece.setInputCloud(cloud_filtered);
	ece.extract(cluster_indices);

	PCDWriter writer;
	int j = 0;
	for (vector<PointIndices>::const_iterator it=cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		PointCloud<PointXYZ>::Ptr cluster_cloud(new PointCloud<PointXYZ>);
		for (vector<int>::const_iterator pit=it->indices.begin(); pit != it->indices.end(); ++pit)
			cluster_cloud->points.push_back(cloud_filtered->points[*pit]);
		cluster_cloud->width = cluster_cloud->points.size();
		cluster_cloud->height = 1;
		cluster_cloud->is_dense = true;

		cout << "PointCloud representing a cluster has: " << cluster_cloud->points.size() << " data points.\n";

		stringstream ss;
		ss << "cloud_cluster_" << j << ".pcd";
		writer.write<PointXYZ>(ss.str(), *cluster_cloud, false);
		j++;
	}

	return 0;
}
Beispiel #15
0
template<typename PointT> int
pcl::PCDWriter::appendBinary(const std::string &file_name, 
                             const pcl::PointCloud<PointT> &cloud)
{
  if(cloud.empty())
  {
    throw pcl::IOException("[pcl::PCDWriter::appendBinary] Input point cloud has no data!");
    return -1;
  }

  if(!boost::filesystem::exists(file_name))
    return writeBinary(file_name, cloud);

  std::ifstream file_istream;
  file_istream.open(file_name.c_str(), std::ifstream::binary);
  if(!file_istream.good())
  {
    throw pcl::IOException("[pcl::PCDWriter::appendBinary] Error opening file for reading");
    return -1;
  }
  file_istream.seekg(0, std::ios_base::end);
  size_t file_size = file_istream.tellg();
  file_istream.close();

  pcl::PCLPointCloud2 tmp_cloud;
  PCDReader reader;
  if(reader.readHeader(file_name, tmp_cloud) != 0)
  {
    throw pcl::IOException("[pcl::PCDWriter::appendBinary] Failed reading header");
    return -1;
  }
  if(tmp_cloud.height != 1 || cloud.height != 1)
  {
    throw pcl::IOException("[pcl::PCDWriter::appendBinary] can't use appendBinary with a point cloud that "
      "has height different than 1!");
    return -1;
  }
  tmp_cloud.width += cloud.width;
  std::ostringstream oss;
  pcl::PointCloud<PointT> tmp_cloud2;
  // copy the header values:
  tmp_cloud2.header = tmp_cloud.header;
  tmp_cloud2.width = tmp_cloud.width;
  tmp_cloud2.height = tmp_cloud.height;
  tmp_cloud2.is_dense = tmp_cloud.is_dense;
  
  oss << PCDWriter::generateHeader(tmp_cloud2, tmp_cloud2.width) << "DATA binary\n";
  size_t data_idx = oss.tellp();
  
#if _WIN32
  HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
  if (h_native_file == INVALID_HANDLE_VALUE)
  {
    throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during CreateFile!");
    return (-1);
  }
#else
  int fd = pcl_open (file_name.c_str (), O_RDWR | O_CREAT | O_APPEND, static_cast<mode_t> (0600));
  if (fd < 0)
  {
    throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during open!");
    return (-1);
  }
#endif
  // Mandatory lock file
  boost::interprocess::file_lock file_lock;
  setLockingPermissions (file_name, file_lock);

  std::vector<pcl::PCLPointField> fields;
  std::vector<int> fields_sizes;
  size_t fsize = 0;
  size_t data_size = 0;
  size_t nri = 0;
  pcl::getFields (cloud, fields);
  // Compute the total size of the fields
  for (size_t i = 0; i < fields.size (); ++i)
  {
    if (fields[i].name == "_")
      continue;

    int fs = fields[i].count * getFieldSize (fields[i].datatype);
    fsize += fs;
    fields_sizes.push_back (fs);
    fields[nri++] = fields[i];
  }
  fields.resize (nri);

  data_size = cloud.points.size () * fsize;

  data_idx += (tmp_cloud.width - cloud.width) * fsize;
  if (data_idx != file_size)
  {
    const char *msg = "[pcl::PCDWriter::appendBinary] The expected data size and the current data size are different!";
    PCL_WARN(msg);
    throw pcl::IOException (msg);
    return -1;
  }

  // Prepare the map
#if _WIN32
  HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL);
  char *map = static_cast<char*>(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size));
  CloseHandle (fm);
#else
  // Stretch the file size to the size of the data
  off_t result = pcl_lseek (fd, getpagesize () + data_size - 1, SEEK_SET);

  if (result < 0)
  {
    pcl_close (fd);
    resetLockingPermissions (file_name, file_lock);
    PCL_ERROR ("[pcl::PCDWriter::appendBinary] lseek errno: %d strerror: %s\n", errno, strerror (errno));

    throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during lseek ()!");
    return (-1);
  }
  // Write a bogus entry so that the new file size comes in effect
  result = static_cast<int> (::write (fd, "", 1));
  if (result != 1)
  {
    pcl_close (fd);
    resetLockingPermissions (file_name, file_lock);
    throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during write ()!");
    return (-1);
  }

  char *map = static_cast<char*> (mmap (0, data_idx + data_size, PROT_WRITE, MAP_SHARED, fd, 0));
  if (map == reinterpret_cast<char*> (-1)) //MAP_FAILED)
  {
    pcl_close (fd);
    resetLockingPermissions (file_name, file_lock);
    throw pcl::IOException ("[pcl::PCDWriter::appendBinary] Error during mmap ()!");
    return (-1);
  }
#endif

  char* out = &map[0] + data_idx;
  // Copy the data
  for (size_t i = 0; i < cloud.points.size (); ++i)
  {
    int nrj = 0;
    for (size_t j = 0; j < fields.size (); ++j)
    {
      memcpy (out, reinterpret_cast<const char*> (&cloud.points[i]) + fields[j].offset, fields_sizes[nrj]);
      out += fields_sizes[nrj++];
    }
  }

  // write the new header:
  std::string header(oss.str());
  memcpy(map, header.c_str(), header.size());


  // If the user set the synchronization flag on, call msync
#if !_WIN32
  if (map_synchronization_)
    msync (map, data_idx + data_size, MS_SYNC);
#endif

  // Unmap the pages of memory
#if _WIN32
  UnmapViewOfFile (map);
#else
  if (munmap (map, (data_idx + data_size)) == -1)
  {
    pcl_close (fd);
    resetLockingPermissions (file_name, file_lock);
    throw pcl::IOException ("[pcl::PCDWriter::writeBinary] Error during munmap ()!");
    return (-1);
  }
#endif
  // Close file
#if _WIN32
  CloseHandle (h_native_file);
#else
  pcl_close (fd);
#endif

  resetLockingPermissions (file_name, file_lock);
  return 0;
}