Ejemplo n.º 1
0
void callback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
  ros::Time whole_start = ros::Time::now();

  ros::Time declare_types_start = ros::Time::now();

  // filter
  pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
  pcl::PassThrough<sensor_msgs::PointCloud2> pass;
  pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
  pcl::ExtractIndices<pcl::Normal> extract_normals;

  // Normal estimation
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
  pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals;

  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());

  // The plane and sphere coefficients
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
  pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ());
  pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());

  // The plane and sphere inliers
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
  pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
  pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ());
  pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());

  // The point clouds
  sensor_msgs::PointCloud2::Ptr voxelgrid_filtered (new sensor_msgs::PointCloud2);
  sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2);
  sensor_msgs::PointCloud2::Ptr rest_output_cloud (new sensor_msgs::PointCloud2);
  sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
  sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2);
  sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);


  // The PointCloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);

  // The cloud normals
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());        // for plane
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ());       // for cylinder
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ());       // for sphere


  ros::Time declare_types_end = ros::Time::now();

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*
   * Voxel grid Filtering
   */
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//  // Create VoxelGrid filtering
//  voxel_grid.setInputCloud (cloud);
//  voxel_grid.setLeafSize (0.01, 0.01, 0.01);
//  voxel_grid.filter (*voxelgrid_filtered);
//
//  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
//  pcl::fromROSMsg (*voxelgrid_filtered, *transformed_cloud);

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*
   * Passthrough Filtering
   */
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//  pass through filter
//  pass.setInputCloud (cloud);
//  pass.setFilterFieldName ("z");
//  pass.setFilterLimits (0, 1.5);
//  pass.filter (*cloud_filtered);
//
//  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
//  pcl::fromROSMsg (*cloud_filtered, *transformed_cloud);


  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*
   * Estimate point normals
   */
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//  ros::Time estimate_start = ros::Time::now();
//
//  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
//  pcl::fromROSMsg (*cloud, *transformed_cloud);
//
//  // Estimate point normals
//  normal_estimation.setSearchMethod (tree);
//  normal_estimation.setInputCloud (transformed_cloud);
//  normal_estimation.setKSearch (50);
//  normal_estimation.compute (*cloud_normals);
//
//  ros::Time estimate_end = ros::Time::now();


  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*
   * Create and processing the plane extraction
   */
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//  ros::Time plane_start = ros::Time::now();
//
//  // Create the segmentation object for the planar model and set all the parameters
//  segmentation_from_normals.setOptimizeCoefficients (true);
//  segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE);
//  segmentation_from_normals.setNormalDistanceWeight (0.1);
//  segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
//  segmentation_from_normals.setMaxIterations (100);
//  segmentation_from_normals.setDistanceThreshold (0.03);
//  segmentation_from_normals.setInputCloud (transformed_cloud);
//  segmentation_from_normals.setInputNormals (cloud_normals);
//
//  // Obtain the plane inliers and coefficients
//  segmentation_from_normals.segment (*inliers_plane, *coefficients_plane);
//  //std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
//
//  // Extract the planar inliers from the input cloud
//  extract_indices.setInputCloud (transformed_cloud);
//  extract_indices.setIndices (inliers_plane);
//  extract_indices.setNegative (false);
//  extract_indices.filter (*cloud_plane);
//
//  pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
//  plane_pub.publish(plane_output_cloud);
//
//  ros::Time plane_end = ros::Time::now();

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  ros::Time plane_start = ros::Time::now();

  pcl::fromROSMsg (*cloud, *transformed_cloud);

  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (1000);
  seg.setDistanceThreshold (0.01);
  seg.setInputCloud (transformed_cloud);
  seg.segment (*inliers_plane, *coefficients_plane);

  extract_indices.setInputCloud(transformed_cloud);
  extract_indices.setIndices(inliers_plane);
  extract_indices.setNegative(false);
  extract_indices.filter(*cloud_plane);

  pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
  plane_pub.publish(plane_output_cloud);
  ros::Time plane_end = ros::Time::now();


  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*
   * Extract rest plane and passthrough filtering
   */
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  ros::Time rest_pass_start = ros::Time::now();

  // Create the filtering object
  // Remove the planar inliers, extract the rest
  extract_indices.setNegative (true);
  extract_indices.filter (*remove_transformed_cloud);
  transformed_cloud.swap (remove_transformed_cloud);

  // publish result of Removal the planar inliers, extract the rest
  pcl::toROSMsg (*transformed_cloud, *rest_output_cloud);
  rest_pub.publish(rest_output_cloud);

  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
//  pcl::fromROSMsg (*rest_output_cloud, *cylinder_cloud);

  // pass through filter
  pass.setInputCloud (rest_output_cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0, 2.5);
  pass.filter (*rest_cloud_filtered);

  ros::Time rest_pass_end = ros::Time::now();

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*
   * for cylinder features
   */
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  /*
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::fromROSMsg (*rest_cloud_filtered, *cylinder_cloud);

  // Estimate point normals
  normal_estimation.setSearchMethod (tree2);
  normal_estimation.setInputCloud (cylinder_cloud);
  normal_estimation.setKSearch (50);
  normal_estimation.compute (*cloud_normals2);

  // Create the segmentation object for sphere segmentation and set all the paopennirameters
  segmentation_from_normals.setOptimizeCoefficients (true);
  segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER);
  segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
  segmentation_from_normals.setNormalDistanceWeight (0.1);
  segmentation_from_normals.setMaxIterations (10000);
  segmentation_from_normals.setDistanceThreshold (0.05);
  segmentation_from_normals.setRadiusLimits (0, 0.5);
  segmentation_from_normals.setInputCloud (cylinder_cloud);
  segmentation_from_normals.setInputNormals (cloud_normals2);

  // Obtain the sphere inliers and coefficients
  segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder);
  //std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

  // Publish the sphere cloud
  extract_indices.setInputCloud (cylinder_cloud);
  extract_indices.setIndices (inliers_cylinder);
  extract_indices.setNegative (false);
  extract_indices.filter (*cylinder_output);

  if (cylinder_output->points.empty ())
     std::cerr << "Can't find the cylindrical component." << std::endl;

  pcl::toROSMsg (*cylinder_output, *cylinder_output_cloud);
  cylinder_pub.publish(cylinder_output_cloud);
  */

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*
   * for sphere features
   */
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


  ros::Time sphere_start = ros::Time::now();

  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::fromROSMsg (*rest_cloud_filtered, *sphere_cloud);

  // Estimate point normals
  normal_estimation.setSearchMethod (tree3);
  normal_estimation.setInputCloud (sphere_cloud);
  normal_estimation.setKSearch (50);
  normal_estimation.compute (*cloud_normals3);

  // Create the segmentation object for sphere segmentation and set all the paopennirameters
  segmentation_from_normals.setOptimizeCoefficients (true);
  //segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
  segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
  segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
  segmentation_from_normals.setNormalDistanceWeight (0.1);
  segmentation_from_normals.setMaxIterations (10000);
  segmentation_from_normals.setDistanceThreshold (0.05);
  segmentation_from_normals.setRadiusLimits (0, 0.2);
  segmentation_from_normals.setInputCloud (sphere_cloud);
  segmentation_from_normals.setInputNormals (cloud_normals3);

  // Obtain the sphere inliers and coefficients
  segmentation_from_normals.segment (*inliers_sphere, *coefficients_sphere);
  //std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;

  // Publish the sphere cloud
  extract_indices.setInputCloud (sphere_cloud);
  extract_indices.setIndices (inliers_sphere);
  extract_indices.setNegative (false);
  extract_indices.filter (*sphere_output);

  if (sphere_output->points.empty ())
     std::cerr << "Can't find the sphere component." << std::endl;

  pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
  sphere_pub.publish(sphere_output_cloud);

  ros::Time sphere_end = ros::Time::now();

  std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
  std::cout << "plane size : " << transformed_cloud->width * transformed_cloud->height << std::endl;
  //std::cout << "plane size : " << cloud_normals->width * cloud_normals->height << std::endl;
  //std::cout << "cylinder size : " << cloud_normals2->width * cloud_normals2->height << std::endl;
  std::cout << "sphere size : " << cloud_normals3->width * cloud_normals3->height << std::endl;

  ros::Time whole_now = ros::Time::now();

  printf("\n");

  std::cout << "whole time         : " << whole_now - whole_start << " sec" << std::endl;
  std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
  //std::cout << "estimate time      : " << estimate_end - estimate_start << " sec" << std::endl;
  std::cout << "plane time         : " << plane_end - plane_start << " sec" << std::endl;
  std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
  std::cout << "sphere time        : " << sphere_end - sphere_start << " sec" << std::endl;

  printf("\n");
}
Ejemplo n.º 2
0
void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& input) {
    ROS_DEBUG("Got new pointcloud.");
    // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
    std_msgs::Header header = input->header;
    pcl::fromROSMsg(*input, *cloud);
    //all the objects needed
    pcl::PCDReader reader;
    pcl::PassThrough<PointT> pass;
    pcl::NormalEstimation<PointT, pcl::Normal> ne;
    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
    pcl::PCDWriter writer;
    pcl::ExtractIndices<PointT> extract;
    pcl::ExtractIndices<pcl::Normal> extract_normals;
    pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
    //data sets
    pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
    pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_sphere (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_sphere (new pcl::PointIndices);
    ROS_DEBUG("PointCloud before filtering has: %i data points.", (int)cloud->points.size());
    
    //filter our NaNs
    pass.setInputCloud (cloud);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0, 2.0);    
    pass.filter (*cloud_filtered);
    ROS_DEBUG("PointCloud after filtering has: %i data points." , (int)cloud_filtered->points.size());
    
    *cloud_filtered = *cloud;//remove the pass through filter basically
    //estimate normal points
    ne.setSearchMethod (tree);
    ne.setInputCloud (cloud_filtered);
    //ne.setKSearch(10);
    ne.setRadiusSearch(0.025);
    ne.compute (*cloud_normals);

    // Create the segmentation object for the planar model and set all the parameters
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
    seg.setNormalDistanceWeight (0.05);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setMaxIterations (100);
    seg.setDistanceThreshold (5.0);
    seg.setInputCloud (cloud_filtered);
    seg.setInputNormals (cloud_normals);
    // Obtain the plane inliers and coefficients
    seg.segment (*inliers_plane, *coefficients_plane);
    std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

    // Extract the planar inliers from the input cloud
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers_plane);
    extract.setNegative (false);
    
    // Remove the planar inliers, extract the rest
    extract.setNegative (true);
    extract.filter (*cloud_filtered2);
    extract_normals.setNegative (true);
    extract_normals.setInputCloud (cloud_normals);
    extract_normals.setIndices (inliers_plane);
    extract_normals.filter (*cloud_normals2);
    
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_SPHERE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setNormalDistanceWeight (0.1);
    seg.setMaxIterations (100);
    seg.setDistanceThreshold (5.0);
    seg.setRadiusLimits (0, 1.0);
    seg.setInputCloud (cloud_filtered2);
    seg.setInputNormals (cloud_normals2);
    
    extract.setInputCloud (cloud_filtered2);
    extract.setIndices (inliers_sphere);
    extract.setNegative (false);
    pcl::PointCloud<PointT>::Ptr sphere_cloud (new pcl::PointCloud<PointT> ());
    extract.filter(*sphere_cloud);
    
    pcl::PointCloud<pcl::PointXYZ> sphere_cloud_in = *sphere_cloud;
    sensor_msgs::PointCloud2 sphere_cloud_out;
    pcl::toROSMsg(sphere_cloud_in, sphere_cloud_out);
    sphere_cloud_out.header = header;
    buoy_cloud_pub.publish(sphere_cloud_out);

}
Ejemplo n.º 3
0
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  bool simple(false), rgb(false), custom_c(false), normals(false),
    shapes(false), viewports(false), interaction_customization(false);
  if (pcl::console::find_argument (argc, argv, "-s") >= 0)
  {
    simple = true;
    std::cout << "Simple visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-c") >= 0)
  {
    custom_c = true;
    std::cout << "Custom colour visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-r") >= 0)
  {
    rgb = true;
    std::cout << "RGB colour visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-n") >= 0)
  {
    normals = true;
    std::cout << "Normals visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-a") >= 0)
  {
    shapes = true;
    std::cout << "Shapes visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-v") >= 0)
  {
    viewports = true;
    std::cout << "Viewports example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-i") >= 0)
  {
    interaction_customization = true;
    std::cout << "Interaction Customization example\n";
  }
  else
  {
    printUsage (argv[0]);
    return 0;
  }

  // ------------------------------------
  // -----Create example point cloud-----
  // ------------------------------------
  pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
  std::cout << "Genarating example point clouds.\n\n";
  // We're going to make an ellipse extruded along the z-axis. The colour for
  // the XYZRGB cloud will gradually go from red to green to blue.
  uint8_t r(255), g(15), b(15);
  for (float z(-1.0); z <= 1.0; z += 0.05)
  {
    for (float angle(0.0); angle <= 360.0; angle += 5.0)
    {
      pcl::PointXYZ basic_point;
      basic_point.x = 0.5 * cosf (pcl::deg2rad(angle));
      basic_point.y = sinf (pcl::deg2rad(angle));
      basic_point.z = z;
      basic_cloud_ptr->points.push_back(basic_point);

      pcl::PointXYZRGB point;
      point.x = basic_point.x;
      point.y = basic_point.y;
      point.z = basic_point.z;
      uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
              static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
      point.rgb = *reinterpret_cast<float*>(&rgb);
      point_cloud_ptr->points.push_back (point);
    }
    if (z < 0.0)
    {
      r -= 12;
      g += 12;
    }
    else
    {
      g -= 12;
      b += 12;
    }
  }
  basic_cloud_ptr->width = (int) basic_cloud_ptr->points.size ();
  basic_cloud_ptr->height = 1;
  point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
  point_cloud_ptr->height = 1;

  // ----------------------------------------------------------------
  // -----Calculate surface normals with a search radius of 0.05-----
  // ----------------------------------------------------------------
  pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
  ne.setInputCloud (point_cloud_ptr);
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
  ne.setSearchMethod (tree);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>);
  ne.setRadiusSearch (0.05);
  ne.compute (*cloud_normals1);

  // ---------------------------------------------------------------
  // -----Calculate surface normals with a search radius of 0.1-----
  // ---------------------------------------------------------------
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
  ne.setRadiusSearch (0.1);
  ne.compute (*cloud_normals2);

  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  if (simple)
  {
    viewer = simpleVis(basic_cloud_ptr);
  }
  else if (rgb)
  {
    viewer = rgbVis(point_cloud_ptr);
  }
  else if (custom_c)
  {
    viewer = customColourVis(basic_cloud_ptr);
  }
  else if (normals)
  {
    viewer = normalsVis(point_cloud_ptr, cloud_normals2);
  }
  else if (shapes)
  {
    viewer = shapesVis(point_cloud_ptr);
  }
  else if (viewports)
  {
    viewer = viewportsVis(point_cloud_ptr, cloud_normals1, cloud_normals2);
  }
  else if (interaction_customization)
  {
    viewer = interactionCustomizationVis();
  }

  //--------------------
  // -----Main loop-----
  //--------------------
  while (!viewer->wasStopped ())
  {
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }
}
Ejemplo n.º 4
0
int
main (int argc, char** argv)
{
  // All the objects needed
  pcl::PCDReader reader;
  pcl::PassThrough<PointT> pass;
  pcl::NormalEstimation<PointT, pcl::Normal> ne;
  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
  pcl::PCDWriter writer;
  pcl::ExtractIndices<PointT> extract;
  pcl::ExtractIndices<pcl::Normal> extract_normals;
  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());

  // Datasets
  pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
  pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
  pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);

  // Read in the cloud data
  reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
  std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;

  // Build a passthrough filter to remove spurious NaNs
  pass.setInputCloud (cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0, 1.5);
  pass.filter (*cloud_filtered);
  std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;

  // Estimate point normals
  ne.setSearchMethod (tree);
  ne.setInputCloud (cloud_filtered);
  ne.setKSearch (50);
  ne.compute (*cloud_normals);

  // Create the segmentation object for the planar model and set all the parameters
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg.setNormalDistanceWeight (0.1);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.03);
  seg.setInputCloud (cloud_filtered);
  seg.setInputNormals (cloud_normals);
  // Obtain the plane inliers and coefficients
  seg.segment (*inliers_plane, *coefficients_plane);
  std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

  // Extract the planar inliers from the input cloud
  extract.setInputCloud (cloud_filtered);
  extract.setIndices (inliers_plane);
  extract.setNegative (false);

  // Write the planar inliers to disk
  pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ());
  extract.filter (*cloud_plane);
  std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
  writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);

  // Remove the planar inliers, extract the rest
  extract.setNegative (true);
  extract.filter (*cloud_filtered2);
  extract_normals.setNegative (true);
  extract_normals.setInputCloud (cloud_normals);
  extract_normals.setIndices (inliers_plane);
  extract_normals.filter (*cloud_normals2);

  // Create the segmentation object for cylinder segmentation and set all the parameters
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_CYLINDER);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (10000);
  seg.setDistanceThreshold (0.05);
  seg.setRadiusLimits (0, 0.1);
  seg.setInputCloud (cloud_filtered2);
  seg.setInputNormals (cloud_normals2);

  // Obtain the cylinder inliers and coefficients
  seg.segment (*inliers_cylinder, *coefficients_cylinder);
  std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

  // Write the cylinder inliers to disk
  extract.setInputCloud (cloud_filtered2);
  extract.setIndices (inliers_cylinder);
  extract.setNegative (false);
  pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
  extract.filter (*cloud_cylinder);
  if (cloud_cylinder->points.empty ()) 
    std::cerr << "Can't find the cylindrical component." << std::endl;
  else
  {
	  std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
	  writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
  }
  return (0);
}
Ejemplo n.º 5
0
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  bool simple(false), rgb(false), custom_c(false), normals(false),
    shapes(false), viewports(false), interaction_customization(false);
  if (pcl::console::find_argument (argc, argv, "-s") >= 0)
  {
    simple = true;
    std::cout << "Simple visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-c") >= 0)
  {
    custom_c = true;
    std::cout << "Custom colour visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-r") >= 0)
  {
    rgb = true;
    std::cout << "RGB colour visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-n") >= 0)
  {
    normals = true;
    std::cout << "Normals visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-a") >= 0)
  {
    shapes = true;
    std::cout << "Shapes visualisation example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-v") >= 0)
  {
    viewports = true;
    std::cout << "Viewports example\n";
  }
  else if (pcl::console::find_argument (argc, argv, "-i") >= 0)
  {
    interaction_customization = true;
    std::cout << "Interaction Customization example\n";
  }
  else
  {
    printUsage (argv[0]);
    return 0;
  }

  // ------------------------------------
  // -----Create example point cloud-----
  // ------------------------------------
  pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
  std::cout << "Genarating example point clouds.\n\n";
  // We're going to make an ellipse extruded along the z-axis. The colour for
  // the XYZRGB cloud will gradually go from red to green to blue.
  uint8_t r(255), g(15), b(15);
  for (float z(-1.0); z <= 1.0; z += 0.05)
  {
    for (float angle(0.0); angle <= 360.0; angle += 5.0)
    {
      pcl::PointXYZ basic_point;
      basic_point.x = 0.5 * cosf (pcl::deg2rad(angle));
      basic_point.y = sinf (pcl::deg2rad(angle));
      basic_point.z = z;
      basic_cloud_ptr->points.push_back(basic_point);

      pcl::PointXYZRGB point;
      point.x = basic_point.x;
      point.y = basic_point.y;
      point.z = basic_point.z;
      uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
              static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
      point.rgb = *reinterpret_cast<float*>(&rgb);
      point_cloud_ptr->points.push_back (point);
    }
    if (z < 0.0)
    {
      r -= 12;
      g += 12;
    }
    else
    {
      g -= 12;
      b += 12;
    }
  }
  basic_cloud_ptr->width = (int) basic_cloud_ptr->points.size ();
  basic_cloud_ptr->height = 1;
  point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
  point_cloud_ptr->height = 1;

  // ----------------------------------------------------------------
  // -----Calculate surface normals with a search radius of 0.05-----
  // ----------------------------------------------------------------
  pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
  ne.setInputCloud (point_cloud_ptr);
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
  ne.setSearchMethod (tree);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>);
  ne.setRadiusSearch (0.05);
  ne.compute (*cloud_normals1);

  // ---------------------------------------------------------------
  // -----Calculate surface normals with a search radius of 0.1-----
  // ---------------------------------------------------------------
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
  ne.setRadiusSearch (0.1);
  ne.compute (*cloud_normals2);

  // ----------------------------------------------------------------
  // -----Load PCD file from Kinect ---------------------------------
  // - TF -----------------------------------------------------------


  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr_kinect (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>& point_cloud_kinect = *point_cloud_ptr_kinect;
  pcl::io::loadPCDFile ("/home/taylor/src/data_pcd/top/kinect_top_rgb.pcd", point_cloud_kinect);

  // --end import --

  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  if (simple)
  {
    viewer = simpleVis(basic_cloud_ptr);
  }
  else if (rgb)
  {
    viewer = rgbVis(point_cloud_ptr_kinect);
  }
  else if (custom_c)
  {
    viewer = customColourVis(point_cloud_ptr_kinect);
  }
  else if (normals)
  {
    viewer = normalsVis(point_cloud_ptr, cloud_normals2);
  }
  else if (shapes)
  {
    viewer = shapesVis(point_cloud_ptr);
  }
  else if (viewports)
  {
    viewer = viewportsVis(point_cloud_ptr, cloud_normals1, cloud_normals2);
  }
  else if (interaction_customization)
  {
    viewer = interactionCustomizationVis();
  }

  //--------------------
  // -----Main loop-----
  //--------------------

  //TF custom
    cout << "begin custom" << endl;   
    viewer->spinOnce(1000);

    viewer->setCameraPosition(0.00, 0.00, -1.25, 0.00, 0.00, 0.625, -0.00, -0.99999, 0.000);
    viewer->setCameraFieldOfView(0.523599);
    viewer->setCameraClipDistances(0.0, 4.0);
    viewer->setSize(1000,1000);

    viewer->updateCamera(); 
    viewer->spinOnce(1000);
    cout << " " << endl;
    cout << "drawing sphere..." << endl;
    pcl::PointXYZ p1;
    p1.x = -0.031;
    p1.y = 0.021;
    p1.z = 0.602;
    viewer->addSphere(p1, 0.01, 1.0, 0.0, 1.0, "PickedPoint", 0);
    
    cout << "end custom" << endl;

 


  //end TF Custom
  while (!viewer->wasStopped ())
  {
    viewer->spinOnce (10000);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));

  }
}
Ejemplo n.º 6
0
// --------------
// -----Main-----
// --------------
int main(int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl17::console::find_argument(argc, argv, "-h") >= 0)
  {
    printUsage(argv[0]);
    return 0;
  }
  bool simple(false), rgb(false), custom_c(false), normals(false), shapes(false), viewports(false),
       interaction_customization(false);
  if (pcl17::console::find_argument(argc, argv, "-s") >= 0)
  {
    simple = true;
    std::cout << "Simple visualisation example\n";
  }
  else if (pcl17::console::find_argument(argc, argv, "-c") >= 0)
  {
    custom_c = true;
    std::cout << "Custom colour visualisation example\n";
  }
  else if (pcl17::console::find_argument(argc, argv, "-r") >= 0)
  {
    rgb = true;
    std::cout << "RGB colour visualisation example\n";
  }
  else if (pcl17::console::find_argument(argc, argv, "-n") >= 0)
  {
    normals = true;
    std::cout << "Normals visualisation example\n";
  }
  else if (pcl17::console::find_argument(argc, argv, "-a") >= 0)
  {
    shapes = true;
    std::cout << "Shapes visualisation example\n";
  }
  else if (pcl17::console::find_argument(argc, argv, "-v") >= 0)
  {
    viewports = true;
    std::cout << "Viewports example\n";
  }
  else if (pcl17::console::find_argument(argc, argv, "-i") >= 0)
  {
    interaction_customization = true;
    std::cout << "Interaction Customization example\n";
  }
  else
  {
    printUsage(argv[0]);
    return 0;
  }

  // ------------------------------------
  // -----Create example point cloud-----
  // ------------------------------------
  pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr basic_cloud_ptr(new pcl17::PointCloud<pcl17::PointXYZRGB>);
  pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr point_cloud_ptr(new pcl17::PointCloud<pcl17::PointXYZRGB>);
  pcl17::PointCloud<pcl17::PointXYZRGB>::Ptr cloud_f(new pcl17::PointCloud<pcl17::PointXYZRGB>);
  pcl17::VoxelGrid<pcl17::PointXYZRGB > sor;
  //pcl17::PLYReader reader;
  //reader.read("box.ply",*point_cloud_ptr,0);
  //pcl17::PointXYZRGB point;

  if (pcl17::io::loadPCDFile<pcl17::PointXYZRGB>(argv[1], *point_cloud_ptr) == -1) //* load the file
  {
    PCL17_ERROR("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }
  basic_cloud_ptr = point_cloud_ptr;
  /*
  std::cout << "Loaded " << point_cloud_ptr->width * point_cloud_ptr->height << " data points" << std::endl;
  cout << "gobbel" << endl;
  sor.setInputCloud(point_cloud_ptr);
  std::cerr << "PointCloud before filtering: " << point_cloud_ptr->width * point_cloud_ptr->height << " data points ("
      << pcl17::getFieldsList(*point_cloud_ptr) << ").";
  std::cout << std::endl;
  sor.setLeafSize(0.01, 0.01, 0.01);
  sor.filter(*cloud_f);
  pcl17::io::savePCDFileASCII("BoxFiltered.pcd", *cloud_f); */
  /* for (size_t i = 0; i < point_cloud_ptr->points.size (); ++i)
   std::cout << "    " << point_cloud_ptr->points[i].x
   << " "    << point_cloud_ptr->points[i].y
   << " "    << point_cloud_ptr->points[i].z << std::endl;
   cout << point_cloud_ptr->width << endl;
   cout << point_cloud_ptr->height << endl;
   for(int i=0;i<point_cloud_ptr->width * point_cloud_ptr->height;i++)
   {
   point=point_cloud_ptr->points.at(i);
   point.r = 255;
   point.g = 0;
   point.b = 0;
   basic_cloud_ptr->points.at(i)=point;
   }
   cout << basic_cloud_ptr->width << endl;
   cout << basic_cloud_ptr->height << endl;
   pcl17::PCDWriter writer;
   writer.write<pcl17::PointXYZRGB> (argv[1], *basic_cloud_ptr, false);
   point_cloud_ptr=basic_cloud_ptr; */
  // ----------------------------------------------------------------
  // -----Calculate surface normals with a search radius of 0.05-----
  // ----------------------------------------------------------------
  pcl17::NormalEstimation<pcl17::PointXYZRGB, pcl17::Normal> ne;
  ne.setInputCloud(point_cloud_ptr);
  pcl17::search::KdTree<pcl17::PointXYZRGB>::Ptr tree(new pcl17::search::KdTree<pcl17::PointXYZRGB>());
  ne.setSearchMethod(tree);
  pcl17::PointCloud<pcl17::Normal>::Ptr cloud_normals1(new pcl17::PointCloud<pcl17::Normal>);
  ne.setRadiusSearch(0.05);
  ne.compute(*cloud_normals1);

  // ---------------------------------------------------------------
  // -----Calculate surface normals with a search radius of 0.1-----
  // ---------------------------------------------------------------
  pcl17::PointCloud<pcl17::Normal>::Ptr cloud_normals2(new pcl17::PointCloud<pcl17::Normal>);
  ne.setRadiusSearch(0.1);
  ne.compute(*cloud_normals2);

  boost::shared_ptr<pcl17::visualization::PCLVisualizer> viewer;
  if (simple)
  {
    viewer = simpleVis(basic_cloud_ptr);
  }
  else if (rgb)
  {
    viewer = rgbVis(point_cloud_ptr);
  }
  else if (custom_c)
  {
    viewer = customColourVis(basic_cloud_ptr);
  }
  else if (normals)
  {
    viewer = normalsVis(point_cloud_ptr, cloud_normals2);
  }
  else if (shapes)
  {
    viewer = shapesVis(point_cloud_ptr);
  }
  else if (viewports)
  {
    viewer = viewportsVis(point_cloud_ptr, cloud_normals1, cloud_normals2);
  }
  else if (interaction_customization)
  {
    viewer = interactionCustomizationVis();
  }

  //--------------------
  // -----Main loop-----
  //--------------------
  while (!viewer->wasStopped())
  {
    viewer->spinOnce(100);
    boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  }
}
Ejemplo n.º 7
0
int
main (int argc, char** argv)
{
  // All the objects needed
  pcl::PCDReader reader;
  pcl::PassThrough<PointT> pass;
  pcl::NormalEstimation<PointT, pcl::Normal> ne;
  
  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 

  pcl::PCDWriter writer;
  pcl::ExtractIndices<PointT> extract;
  pcl::ExtractIndices<pcl::Normal> extract_normals;
  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());

  // Datasets
  pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
  pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
  pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);

  // Read in the cloud data
  std::vector<int> filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  if (filenames.size() == 0) {
    PCL_ERROR ("No pcd files provided");
    return 0;
  }
  if (pcl::io::loadPCDFile<PointType> (argv[filenames[0]], *cloud) == -1) {
    PCL_ERROR ("Couldn't read file %s.pcd \n", argv[filenames[0]]);
  }

  std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;

  // Estimate point normals
  ne.setSearchMethod (tree);
  ne.setInputCloud (cloud);
  ne.setKSearch (10);
  ne.compute (*cloud_normals);

  // Create the segmentation object for cylinder segmentation and set all the parameters
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_CYLINDER);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (10000);
  seg.setDistanceThreshold (0.05);
  seg.setRadiusLimits (0, 0.1);
  seg.setInputCloud (cloud);
  seg.setInputNormals (cloud_normals);

  // Obtain the cylinder inliers and coefficients
  seg.segment (*inliers_cylinder, *coefficients_cylinder);
  std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

  // Write the cylinder inliers to disk
  extract.setInputCloud (cloud);
  extract.setIndices (inliers_cylinder);
  extract.setNegative (false);
  pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
  extract.filter (*cloud_cylinder);
  if (cloud_cylinder->points.empty ()) 
    std::cerr << "Can't find the cylindrical component." << std::endl;
  else
  {
	  std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
	  writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
  }
  return (0);
}
int main (int argc, char** argv)
{
	
  
  // Zmienne ktore uzywamy do segmentacji, filtracji, odczytu i zapisu pliku.
  pcl::PCDReader reader;
  pcl::PassThrough<PointT> pass;
  pcl::NormalEstimation<PointT, pcl::Normal> ne;
  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
  pcl::PCDWriter writer;
  pcl::ExtractIndices<PointT> extract;
  pcl::ExtractIndices<pcl::Normal> extract_normals;
  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
  pcl::visualization::CloudViewer viewer ("Cylinder Model Segmentation");
  // Zmienne ktore przechowuja kolejno chmury naszych punktów
  pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
  pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
  pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);

  // Wczytanie chmury punktów
  reader.read ("test_pcd.pcd", *cloud);
  std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;

  // Przefiltorwanie chmury punktów w celu usuniecia "fa³szywych" punktów (NaN)
  pass.setInputCloud (cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0, 2.8);
  pass.filter (*cloud_filtered);
  std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;

  // Obliczenie normalnych dla punktów w chmurze
  ne.setSearchMethod (tree);
  ne.setInputCloud (cloud_filtered);
  ne.setKSearch (50);
  ne.compute (*cloud_normals);

  // Stworzenie obiektu do segmentacji planarnej, ustawienie odpowiednich parametrów
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg.setNormalDistanceWeight (0.1);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.03);
  seg.setInputCloud (cloud_filtered);
  seg.setInputNormals (cloud_normals);
  // Segmentacja..
  seg.segment (*inliers_plane, *coefficients_plane);
  std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

  // Wy³uskanie obiektu ktory segmentowalismy
  extract.setInputCloud (cloud_filtered);
  extract.setIndices (inliers_plane);
  extract.setNegative (true);
  extract.filter (*cloud_filtered2);

  extract_normals.setNegative (true);
  extract_normals.setInputCloud (cloud_normals);
  extract_normals.setIndices (inliers_plane);
  extract_normals.filter (*cloud_normals2);

  // Utworzenie obiektu do segmentacji cylindrycznej, ustawienie odpowiednich parametrów
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_CYLINDER);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (10000);
  // Maksymalna odleglosc pomiedzy punktam a prost¹ ktora wyznaczana jest przy segmentacji moze wynosic 33 cm
  seg.setDistanceThreshold (0.33);
  // Maksymalny promien cylindra moze miec 40 cm
  seg.setRadiusLimits (0, 0.45);
  seg.setInputCloud (cloud_filtered2);
  seg.setInputNormals (cloud_normals2);

  // Segmentacja...
  seg.segment (*inliers_cylinder, *coefficients_cylinder);
  std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

  // Zapisz wynik do pliku na dysku
  extract.setInputCloud (cloud_filtered2);
  extract.setIndices (inliers_cylinder);
  extract.setNegative (false);
  pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
  extract.filter (*cloud_cylinder);
  if (cloud_cylinder->points.empty ()) 
    std::cerr << "Can't find the cylindrical component." << std::endl;
  else
  {
	  std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
	  writer.write ("test_pcd_cylinder.pcd", *cloud_cylinder, false);
	     viewer.showCloud (cloud_cylinder);
	   while (!viewer.wasStopped ())
	   {
	   }
  }

  return (0);

}