Exemplo n.º 1
0
void downsampling(double scale)
{
    CloudConstPtr pCloudConst = (CloudConstPtr)pCloud_input;

    pcl::VoxelGrid<PointT> vg;
    vg.setInputCloud(pCloudConst);
    vg.setLeafSize(scale, scale, scale); // down sampling using a leaf size of 'scale'
    vg.filter(*pCloud);

    pCloud_input.reset(new Cloud);
    pCloud_input = pCloud;
    pCloud.reset(new Cloud);
}
Exemplo n.º 2
0
/* ---[ */
int
main (int argc, char** argv)
{
  // Check whether we want to enable debug mode
  bool debug = false;
  parse_argument (argc, argv, "-debug", debug);
  if (debug)
    setVerbosityLevel (L_DEBUG);

  parse_argument (argc, argv, "-rejection", rejection);
  parse_argument (argc, argv, "-visualization", visualize);
  if (visualize)
    vis.reset (new PCLVisualizer ("Registration example"));

  // Parse the command line arguments for .pcd and .transform files
  std::vector<int> p_pcd_file_indices, p_tr_file_indices;
  p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_pcd_file_indices.size () != 2)
  {
    print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }
  p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform");
  if (p_tr_file_indices.size () != 1)
  {
    print_error ("Need one output transform file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }
  
  // Load the files
  print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]);
  src.reset (new PointCloud<PointT>);
  tgt.reset (new PointCloud<PointT>);
  if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1)
  {
    print_error ("Error reading the input files!\n");
    return (-1);
  }

  // Compute the best transformtion
  Eigen::Matrix4d transform;
  icp (src, tgt, transform);

  saveTransform (argv[p_tr_file_indices[0]], transform);

  cerr.precision (15);
  std::cerr << transform << std::endl;
}
Exemplo n.º 3
0
void planeExtraction(int nPlane)
{
    CloudConstPtr pCloudConst = (CloudConstPtr)pCloud_input;

    // Create the segmentation object for the planar model and set all the parameters
    pcl::SACSegmentation<PointT> seg;
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    //    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setMaxIterations (100);
    seg.setDistanceThreshold (0.02);

    // plane segmentation
    for(int i=0;i<nPlane;i++)
    {
        // Segment the largest planar component from the remaining cloud
        seg.setInputCloud(pCloud_input);
        seg.segment (*inliers, *coefficients); //*
        if (inliers->indices.size () == 0)
        {
            std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }

        // Extract the planar inliers from the input cloud
        pcl::ExtractIndices<PointT> extract;
        extract.setInputCloud (pCloudConst);
        extract.setIndices (inliers);
        extract.setNegative (false);

        // Write the planar inliers to disk
        CloudPtr cloud_plane;
        cloud_plane.reset(new Cloud);
        extract.filter (*cloud_plane); //*
//        clouds_plane.push_back(cloud_plane);
        //      std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;

        // Remove the planar inliers, extract the rest
        extract.setNegative (true);
        extract.filter (*pCloud); //*
    }

    pCloud_input.reset(new Cloud);
    pCloud_input = pCloud;
    pCloud.reset(new Cloud);
}
Exemplo n.º 4
0
void ObjectDetector::processCloud(const CloudConstPtr& cloud)
{
	cloud_pass_through_.reset(new Cloud);
	// Computation goes here
	pass_through_.setInputCloud(cloud);
	pass.filter(*cloud_pass_through_);
	
	// Estimate 3d convex hull
	pcl::ConvexHull<PointType> hr;
	hr.setInputCloud(cloud_pass_through_);
	cloud_hull_.reset(new Cloud);
	hr.reconstruct(*cloud_hull_, vertices_);
	
	cloud_ = cloud;
	new_cloud_ = true;
}
Exemplo n.º 5
0
//OpenNI Grabber's cloud Callback function
void
cloud_cb (const CloudConstPtr &cloud)
{
  boost::mutex::scoped_lock lock (mtx_);
  cloud_pass_.reset (new Cloud);
  cloud_pass_downsampled_.reset (new Cloud);
  filterPassThrough (cloud, *cloud_pass_);
  gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);

  if(counter < 10){
	counter++;
  }else{
  	//Track the object
	tracker_->setInputCloud (cloud_pass_downsampled_);
	tracker_->compute ();
	new_cloud_ = true;
  }
}
Exemplo n.º 6
0
void workingspace()
{
    double top = param_workspace_y + param_workspace_height/2;
    double bottom = param_workspace_y - param_workspace_height/2;
    double left = param_workspace_x - param_workspace_width/2;
    double right = param_workspace_x + param_workspace_width/2;
    double zbottom = param_workspace_z;
    double ztop = param_workspace_z + param_workspace_zheight;

    for(int i=0;i<pCloud_input->points.size();i++){
        PointT temp_point = pCloud_input->points[i];
        // cut off
        if(temp_point.x<right && temp_point.x>left && temp_point.y<top && temp_point.y>bottom && temp_point.z>zbottom && temp_point.z<ztop)
                pCloud->points.push_back(temp_point);
    }
    pCloud_input.reset(new Cloud);
    pCloud_input = pCloud;
    pCloud.reset(new Cloud);

    workspace.header.frame_id = param_frame_id.data();
    workspace.header.stamp = ros::Time::now();
    pub_workspace.publish (workspace);
}
  void
      run ()
      {
        CloudPtr filecloud;
        pcl::Grabber* interface;
        if(filename_.empty()) {
          interface = new pcl::OpenNIGrabber (device_id_);

          boost::function<void (const CloudConstPtr&)> f = boost::bind (&PclPyramid::cloud_cb_, this, _1);
          boost::signals2::connection c = interface->registerCallback (f);

          interface->start ();
        }
        else 
        {
          pcd_cloud.reset (new pcl::PCLPointCloud2);
          if(pcd.read (filename_, *pcd_cloud, origin, orientation, version) < 0)
            cout << "file read failed" << endl;
          filecloud.reset (new Cloud);
          pcl::fromPCLPointCloud2(*pcd_cloud, *filecloud);
          cloud_  = filecloud;
        }


#ifdef NOVIEWER        
        if(!filename_.empty()) 
          get();
        else 
          while(TRUE)
          {
            if (cloud_)
              get();
            boost::this_thread::sleep (boost::posix_time::microseconds (10000));
          }
#else
        while ( !viewer.wasStopped () )
        {
          if (cloud_)
          {
            //the call to get() sets the cloud_ to null;
            viewer.showCloud (get ());
          }
          boost::this_thread::sleep (boost::posix_time::microseconds (10000));
        }
#endif

        if(filename_.empty()) {
          interface->stop ();
        }
      }
Exemplo n.º 8
0
/* ---[ */
int
main (int argc, char** argv)
{
  if (argc < 3)
  {
    std::cerr << "No test file given. Please download `bun0.pcd` and `milk.pcd` pass its path to the test." << std::endl;
    return (-1);
  }

  if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
  {
    std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  CloudPtr milk_loaded(new PointCloud<PointXYZ>());
  if (loadPCDFile<PointXYZ> (argv[2], *milk_loaded) < 0)
  {
    std::cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  indices.resize (cloud.points.size ());
  for (size_t i = 0; i < indices.size (); ++i)
  {
    indices[i] = static_cast<int>(i);
  }

  tree.reset (new search::KdTree<PointXYZ> (false));
  tree->setInputCloud (cloud.makeShared ());

  cloud_milk.reset(new PointCloud<PointXYZ>());
  CloudPtr grid;
  pcl::VoxelGrid < pcl::PointXYZ > grid_;
  grid_.setInputCloud (milk_loaded);
  grid_.setLeafSize (leaf_size_, leaf_size_, leaf_size_);
  grid_.filter (*cloud_milk);

  tree_milk.reset (new search::KdTree<PointXYZ> (false));
  tree_milk->setInputCloud (cloud_milk);

  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}
Exemplo n.º 9
0
void cb_rgb(const pcl::PCLPointCloud2ConstPtr& input)
{
    static int cnt = 0;

    // input
    pCloud_input.reset(new Cloud);
    pCloud.reset(new Cloud);
    pcl::fromPCLPointCloud2(*input, *pCloud_input);
    if(isdebug) cout<<"I heard RGB, # of points: "<<pCloud_input->points.size()<<endl;

    // downsampling
    lastT = pcl::getTime ();
    downsampling(cont_sampling);
    if(isdebug) cout<<"downsampling computation time : "<<pcl::getTime()-lastT<<endl;
    if(isdebug) cout<<"downsampling end, # of points: "<<pCloud_input->points.size()<<endl;

    // transform to the given frame
    lastT = pcl::getTime ();
    transform(param_frame_id);
    if(isdebug) cout<<"transform computation time : "<<pcl::getTime()-lastT<<endl;

    // workspace
    lastT = pcl::getTime ();
    workingspace();
    if(isdebug) cout<<"workingspace computation time : "<<pcl::getTime()-lastT<<endl;
    if(isdebug) cout<<"workingspace end, # of points: "<<pCloud_input->points.size()<<endl;

//    // planeExtraction
//    lastT = pcl::getTime ();
//    planeExtraction(1);
//    if(isdebug) cout<<"planeExtraction computation time : "<<pcl::getTime()-lastT<<endl;
//    if(isdebug) cout<<"planeExtraction end, # of points: "<<pCloud_input->points.size()<<endl;

    // tracking
    lastT = pcl::getTime ();
    pctracking->run(pCloud_input);
    nowT = pcl::getTime ();
    if(isdebug) cout<<"total tracking computation time : "<<nowT-lastT<<endl;

    // publish filtered pointcloud
    pcl::PCLPointCloud2 output_filtered;
    pcl::toPCLPointCloud2(*pctracking->getFilteredPC(), output_filtered);
    output_filtered.header.frame_id=param_frame_id.data();    
    pub_filtered.publish (output_filtered);

    // publish segmented pointcloud
    pcl::PCLPointCloud2 output_segmented;
    pcl::toPCLPointCloud2(*pctracking->getSegmentedPC(), output_segmented);
    output_segmented.header.frame_id=param_frame_id.data();
    pub_segmented.publish (output_segmented);

    // publish model pointcloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut_model;
    pCloudOut_model.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    pctracking->object_tracks->toPointCloudXYZI_model(*pCloudOut_model);
    pcl::PCLPointCloud2 output_model;
    pcl::toPCLPointCloud2(*pCloudOut_model, output_model);
    output_model.header.frame_id=param_frame_id.data();
    pub_objectmodel.publish (output_model);

    // output tracks pointcloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut;
    pCloudOut.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    pctracking->object_tracks->toPointCloudXYZI(*pCloudOut);
    //    ROS_INFO("# of track points: %d", pCloudOut->points.size());

    // publish pointcloud(currentT) of tracks
    pcl::PCLPointCloud2 output;
    pcl::toPCLPointCloud2(*pCloudOut, output);
    output.header.frame_id=param_frame_id.data();
    pub_track.publish (output);


    //    // publish lastmodel
    //    pcl::PCLPointCloud2 output_lastmodel;
    //    pcl::toPCLPointCloud2(*pctracking->getLastModel(), output_lastmodel);
    //    output_lastmodel.header.frame_id=param_frame_id.data();
    //    pub_lastmodel.publish (output_lastmodel);

    //    // publish particles
    //    pcl::PCLPointCloud2 output_particles;
    //    pcl::toPCLPointCloud2(*pctracking->getParticles(), output_particles);
    //    output_particles.header.frame_id=param_frame_id.data();
    //    pub_particles.publish (output_particles);


    // publish gaussians
    pub_gaussians.publish(pctracking->object_tracks->oldGaussians());
    pub_gaussians.publish(pctracking->object_tracks->toMarkerGaussians());

    // publish gmms
//    pub_gmms.publish(pctracking->object_tracks->toMarkerGMMs());
//    pub_edges.publish(pctracking->object_tracks->toMarkerEdges());

    // publish delete id marker of deleted tracks
    pub_trackID.publish(pctracking->object_tracks->oldMarkerIDs());
    pub_trackID.publish(pctracking->object_tracks->toMarkerIDs());

    pCloudOut.reset();
    cnt++;

}
Exemplo n.º 10
0
    void
    segment (const PointT &picked_point, 
             int picked_idx,
             PlanarRegion<PointT> &region,
             PointIndices &indices,
             CloudPtr &object)
    {
      // First frame is segmented using an organized multi plane segmentation approach from points and their normals
      if (!first_frame_)
        return;

      // Estimate normals in the cloud
      PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
      ne_.setInputCloud (search_.getInputCloud ());
      ne_.compute (*normal_cloud);

      plane_comparator_->setDistanceMap (ne_.getDistanceMap ());

      // Segment out all planes
      mps_.setInputNormals (normal_cloud);
      mps_.setInputCloud (search_.getInputCloud ());

      // Use one of the overloaded segmentAndRefine calls to get all the information that we want out
      vector<PlanarRegion<PointT> > regions;
      vector<ModelCoefficients> model_coefficients;
      vector<PointIndices> inlier_indices;  
      PointCloud<Label>::Ptr labels (new PointCloud<Label>);
      vector<PointIndices> label_indices;
      vector<PointIndices> boundary_indices;
      mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
      PCL_DEBUG ("Number of planar regions detected: %zu for a cloud of %zu points and %zu normals.\n", regions.size (), search_.getInputCloud ()->points.size (), normal_cloud->points.size ());

      double max_dist = numeric_limits<double>::max ();
      // Compute the distances from all the planar regions to the picked point, and select the closest region
      int idx = -1;
      for (size_t i = 0; i < regions.size (); ++i)
      {
        double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ()); 
        if (dist < max_dist)
        {
          max_dist = dist;
          idx = static_cast<int> (i);
        }
      }

      PointIndices::Ptr plane_boundary_indices;
      // Get the plane that holds the object of interest
      if (idx != -1)
      {
        region = regions[idx]; 
        plane_indices_.reset (new PointIndices (inlier_indices[idx]));
        plane_boundary_indices.reset (new PointIndices (boundary_indices[idx]));
      }

      // Segment the object of interest
      if (plane_boundary_indices && !plane_boundary_indices->indices.empty ())
      {
        object.reset (new Cloud);
        segmentObject (picked_idx, search_.getInputCloud (), plane_indices_, plane_boundary_indices, *object);

        // Save to disk
        //pcl::io::saveTARPointCloud ("output.ltm", *object, "object.pcd");
      }
      first_frame_ = false;
    }
Exemplo n.º 11
0
void VisMotCoord::getPCScene(CloudPtr &pc_out)
{
    pc_out.reset(new Cloud);
    *pc_out = *scene.getPCScene();
}
Exemplo n.º 12
0
int
main (int argc, char** argv)
{
  if (argc < 3)
    {
      PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]);
      exit (1);
    }

  //read pcd file
  target_cloud.reset(new Cloud());
  if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){
    std::cout << "pcd file not found" << std::endl;
    exit(-1);
  }

  std::string device_id = std::string (argv[1]);  

  counter = 0;

  //Set parameters
  new_cloud_  = false;
  downsampling_grid_size_ =  0.002;

  std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
  default_step_covariance[3] *= 40.0;
  default_step_covariance[4] *= 40.0;
  default_step_covariance[5] *= 40.0;

  std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
  std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);

  KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT>::Ptr tracker
    (new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (8));

  ParticleT bin_size;
  bin_size.x = 0.1f;
  bin_size.y = 0.1f;
  bin_size.z = 0.1f;
  bin_size.roll = 0.1f;
  bin_size.pitch = 0.1f;
  bin_size.yaw = 0.1f;


  //Set all parameters for  KLDAdaptiveParticleFilterOMPTracker
  tracker->setMaximumParticleNum (1000);
  tracker->setDelta (0.99);
  tracker->setEpsilon (0.2);
  tracker->setBinSize (bin_size);

  //Set all parameters for  ParticleFilter
  tracker_ = tracker;
  tracker_->setTrans (Eigen::Affine3f::Identity ());
  tracker_->setStepNoiseCovariance (default_step_covariance);
  tracker_->setInitialNoiseCovariance (initial_noise_covariance);
  tracker_->setInitialNoiseMean (default_initial_mean);
  tracker_->setIterationNum (1);
  tracker_->setParticleNum (600);
  tracker_->setResampleLikelihoodThr(0.00);
  tracker_->setUseNormal (false);


  //Setup coherence object for tracking
  ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence
    (new ApproxNearestPairPointCloudCoherence<RefPointType>);

  DistanceCoherence<RefPointType>::Ptr distance_coherence
    (new DistanceCoherence<RefPointType>);
  coherence->addPointCoherence (distance_coherence);

  pcl::search::Octree<RefPointType>::Ptr search (new pcl::search::Octree<RefPointType> (0.01));
  coherence->setSearchMethod (search);
  coherence->setMaximumDistance (0.01);

  tracker_->setCloudCoherence (coherence);

  //prepare the model of tracker's target
  Eigen::Vector4f c;
  Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
  CloudPtr transed_ref (new Cloud);
  CloudPtr transed_ref_downsampled (new Cloud);

  pcl::compute3DCentroid<RefPointType> (*target_cloud, c);
  trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
  pcl::transformPointCloud<RefPointType> (*target_cloud, *transed_ref, trans.inverse());
  gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);

  //set reference model and trans
  tracker_->setReferenceCloud (transed_ref_downsampled);
  tracker_->setTrans (trans);

  //Setup OpenNIGrabber and viewer
  pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer");
  pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
  boost::function<void (const CloudConstPtr&)> f =
    boost::bind (&cloud_cb, _1);
  interface->registerCallback (f);
    
  viewer_->runOnVisualizationThread (boost::bind(&viz_cb, _1), "viz_cb");

  //Start viewer and object tracking
  interface->start();
  while (!viewer_->wasStopped ())
    std::this_thread::sleep_for(1s);
  interface->stop();
}