示例#1
0
void SceneCloudView::generateCloud(KinfuTracker& kinfu, bool integrate_colors)
{
  viewer_pose_ = kinfu.getCameraPose();

  ScopeTimeT time ("PointCloud Extraction");
  cout << "\nGetting cloud... " << flush;

  valid_combined_ = false;
  bool valid_extracted_ = false;

  if (extraction_mode_ != GPU_Connected6)     // So use CPU
  {
    kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
  }
  else
  {
    DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);

    if(extracted.size() > 0){
        valid_extracted_ = true;

        extracted.download (cloud_ptr_->points);
        cloud_ptr_->width = (int)cloud_ptr_->points.size ();
        cloud_ptr_->height = 1;

        if (integrate_colors)
        {
          kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
          point_colors_device_.download(point_colors_ptr_->points);
          point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
          point_colors_ptr_->height = 1;
          //pcl::gpu::mergePointRGB(extracted, point_colors_device_, combined_color_device_);
          //combined_color_device_.download (combined_color_ptr_->points);
        }
        else
          point_colors_ptr_->points.clear();
        combined_color_ptr_->clear();
        generateXYZRGB(cloud_ptr_, point_colors_ptr_, combined_color_ptr_);

    }else{
        valid_extracted_ = false;
        cout << "Failed to Extract Cloud " << endl;

    }
  }

  cout << "Done.  Cloud size: " << cloud_ptr_->points.size () / 1000 << "K" << endl;

}
示例#2
0
  void
  show (KinfuTracker& kinfu, bool integrate_colors)
  {
    viewer_pose_ = kinfu.getCameraPose();

    ScopeTimeT time ("PointCloud Extraction");
    cout << "\nGetting cloud... " << flush;

    valid_combined_ = false;

    if (extraction_mode_ != GPU_Connected6)     // So use CPU
    {
      kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
    }
    else
    {
      DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);             

      if (compute_normals_)
      {
        kinfu.volume().fetchNormals (extracted, normals_device_);
        pcl::gpu::mergePointNormal (extracted, normals_device_, combined_device_);
        combined_device_.download (combined_ptr_->points);
        combined_ptr_->width = (int)combined_ptr_->points.size ();
        combined_ptr_->height = 1;

        valid_combined_ = true;
      }
      else
      {
        extracted.download (cloud_ptr_->points);
        cloud_ptr_->width = (int)cloud_ptr_->points.size ();
        cloud_ptr_->height = 1;
      }

      if (integrate_colors)
      {
        kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
        point_colors_device_.download(point_colors_ptr_->points);
        point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
        point_colors_ptr_->height = 1;
      }
      else
        point_colors_ptr_->points.clear();
    }
    size_t points_size = valid_combined_ ? combined_ptr_->points.size () : cloud_ptr_->points.size ();
    cout << "Done.  Cloud size: " << points_size / 1000 << "K" << endl;

    cloud_viewer_.removeAllPointClouds ();    
    if (valid_combined_)
    {
      visualization::PointCloudColorHandlerRGBHack<PointNormal> rgb(combined_ptr_, point_colors_ptr_);
      cloud_viewer_.addPointCloud<PointNormal> (combined_ptr_, rgb, "Cloud");
      cloud_viewer_.addPointCloudNormals<PointNormal>(combined_ptr_, 50);
    }
    else
    {
      visualization::PointCloudColorHandlerRGBHack<PointXYZ> rgb(cloud_ptr_, point_colors_ptr_);
      cloud_viewer_.addPointCloud<PointXYZ> (cloud_ptr_, rgb);
    }    
  }