Beispiel #1
0
boost::shared_ptr<pcl::PolygonMesh> convertToMesh(const DeviceArray<PointXYZ>& triangles)
{ 
  if (triangles.empty())
      return boost::shared_ptr<pcl::PolygonMesh>();

  pcl::PointCloud<pcl::PointXYZ> cloud;
  cloud.width  = (int)triangles.size();
  cloud.height = 1;
  triangles.download(cloud.points);
  
  boost::shared_ptr<pcl::PolygonMesh> mesh_ptr( new pcl::PolygonMesh() ); 
  pcl::toPCLPointCloud2(cloud, mesh_ptr->cloud);
      
  mesh_ptr->polygons.resize (triangles.size() / 3);
  for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
  {
    pcl::Vertices v;
    v.vertices.push_back(i*3+0);
    v.vertices.push_back(i*3+2);
    v.vertices.push_back(i*3+1);              
    mesh_ptr->polygons[i] = v;
  }    
  return mesh_ptr;
  
  cout << mesh_ptr->polygons.size () << " plys\n";
}
Beispiel #2
0
boost::shared_ptr<pcl::PolygonMesh> convertToMesh(const DeviceArray<PointXYZ>& triangles)
{
    if (triangles.empty())
    {
        std::cerr << "kinfu_util::convertToMesh(): triangles empty...returning null..." << std::endl;
        return boost::shared_ptr<pcl::PolygonMesh>();
    }

    pcl::PointCloud<pcl::PointXYZ> cloud;
    cloud.width  = (int)triangles.size();
    cloud.height = 1;
    triangles.download(cloud.points);

    boost::shared_ptr<pcl::PolygonMesh> mesh_ptr( new pcl::PolygonMesh() );
    pcl::toROSMsg(cloud, mesh_ptr->cloud);

    mesh_ptr->polygons.resize (triangles.size() / 3);
    for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
    {
        pcl::Vertices v;
        v.vertices.push_back(i*3+0);
        v.vertices.push_back(i*3+2);
        v.vertices.push_back(i*3+1);
        mesh_ptr->polygons[i] = v;
    }
    return mesh_ptr;
}
Beispiel #3
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;

}
Beispiel #4
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);
    }    
  }
Beispiel #5
0
void
pcl::gpu::kinfuLS::CyclicalBuffer::performShift (const TsdfVolume::Ptr volume, const pcl::PointXYZ &target_point, const bool last_shift)
{
  // compute new origin and offsets
  int offset_x, offset_y, offset_z;
  computeAndSetNewCubeMetricOrigin (target_point, offset_x, offset_y, offset_z);

  // extract current slice from the TSDF volume (coordinates are in indices! (see fetchSliceAsCloud() )
  DeviceArray<PointXYZ> points;
  DeviceArray<float> intensities;
  int size;
  if(!last_shift)
  {
    size = volume->fetchSliceAsCloud (cloud_buffer_device_xyz_, cloud_buffer_device_intensities_, &buffer_, offset_x, offset_y, offset_z);
  }
  else
  {
    size = volume->fetchSliceAsCloud (cloud_buffer_device_xyz_, cloud_buffer_device_intensities_, &buffer_, buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1);
  }
  points = DeviceArray<PointXYZ> (cloud_buffer_device_xyz_.ptr (), size);
  intensities = DeviceArray<float> (cloud_buffer_device_intensities_.ptr(), size);

  PointCloud<PointXYZI>::Ptr current_slice (new PointCloud<PointXYZI>);
  PointCloud<PointXYZ>::Ptr current_slice_xyz (new PointCloud<PointXYZ>);
  PointCloud<PointIntensity>::Ptr current_slice_intensities (new PointCloud<PointIntensity>);

  // Retrieving XYZ
  points.download (current_slice_xyz->points);
  current_slice_xyz->width = (int) current_slice_xyz->points.size ();
  current_slice_xyz->height = 1;

  // Retrieving intensities
  // TODO change this mechanism by using PointIntensity directly (in spite of float)
  // when tried, this lead to wrong intenisty values being extracted by fetchSliceAsCloud () (padding pbls?)
  std::vector<float , Eigen::aligned_allocator<float> > intensities_vector;
  intensities.download (intensities_vector);
  current_slice_intensities->points.resize (current_slice_xyz->points.size ());
  for(int i = 0 ; i < current_slice_intensities->points.size () ; ++i)
    current_slice_intensities->points[i].intensity = intensities_vector[i];

  current_slice_intensities->width = (int) current_slice_intensities->points.size ();
  current_slice_intensities->height = 1;

  // Concatenating XYZ and Intensities
  pcl::concatenateFields (*current_slice_xyz, *current_slice_intensities, *current_slice);
  current_slice->width = (int) current_slice->points.size ();
  current_slice->height = 1;

  // transform the slice from local to global coordinates
  Eigen::Affine3f global_cloud_transformation;
  global_cloud_transformation.translation ()[0] = buffer_.origin_GRID_global.x;
  global_cloud_transformation.translation ()[1] = buffer_.origin_GRID_global.y;
  global_cloud_transformation.translation ()[2] = buffer_.origin_GRID_global.z;
  global_cloud_transformation.linear () = Eigen::Matrix3f::Identity ();
  transformPointCloud (*current_slice, *current_slice, global_cloud_transformation);

  // retrieve existing data from the world model
  PointCloud<PointXYZI>::Ptr previously_existing_slice (new  PointCloud<PointXYZI>);
  world_model_.getExistingData (buffer_.origin_GRID_global.x, buffer_.origin_GRID_global.y, buffer_.origin_GRID_global.z,
                                offset_x, offset_y, offset_z,
                                buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1,
                                *previously_existing_slice);

  //replace world model data with values extracted from the TSDF buffer slice
  world_model_.setSliceAsNans (buffer_.origin_GRID_global.x, buffer_.origin_GRID_global.y, buffer_.origin_GRID_global.z,
                               offset_x, offset_y, offset_z,
                               buffer_.voxels_size.x, buffer_.voxels_size.y, buffer_.voxels_size.z);


  PCL_INFO ("world contains %d points after update\n", world_model_.getWorldSize ());
  world_model_.cleanWorldFromNans ();
  PCL_INFO ("world contains %d points after cleaning\n", world_model_.getWorldSize ());

  // clear buffer slice and update the world model
  pcl::device::kinfuLS::clearTSDFSlice (volume->data (), &buffer_, offset_x, offset_y, offset_z);

  // insert current slice in the world if it contains any points
  if (current_slice->points.size () != 0) {
    world_model_.addSlice(current_slice);
  }

  // shift buffer addresses
  shiftOrigin (volume, offset_x, offset_y, offset_z);

  // push existing data in the TSDF buffer
  if (previously_existing_slice->points.size () != 0 ) {
    volume->pushSlice(previously_existing_slice, getBuffer () );
  }
}