Exemplo 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");
}
Exemplo n.º 2
0
// Align a collection of object templates to a sample point cloud
void cloud_cb( const sensor_msgs::PointCloud2ConstPtr& input )
{
  if( controllerState != 1 )
    return;

    //--- Convert Incoming Cloud --- //

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud( new pcl::PointCloud<pcl::PointXYZ> );
    pcl::fromROSMsg( *input, *cloud );

	
	// --- Z-Filter And Downsample Cloud --- //

	// Preprocess the cloud by removing distant points
	pcl::PassThrough<pcl::PointXYZ> pass_z;
	pass_z.setInputCloud( cloud );
	pass_z.setFilterFieldName( "z" );
	pass_z.setFilterLimits( 0, 1.75 );
	pass_z.filter( *cloud );

	pcl::PassThrough<pcl::PointXYZ> pass_y;
	pass_y.setInputCloud( cloud );
	pass_y.setFilterFieldName("y");
	pass_y.setFilterLimits( -0.5, 0.2 );
	pass_y.filter( *cloud );
	
	pcl::PassThrough<pcl::PointXYZ> pass_x;
	pass_x.setInputCloud( cloud );
	pass_x.setFilterFieldName("x");
	pass_x.setFilterLimits( -0.5, 0.5 );
	pass_x.filter( *cloud );

	// It is possible to not have any points after z-filtering (ex. if we are looking up).
	// Just bail if there is nothing left.
	if( cloud->points.size() == 0 )
		return;

	//visualize( cloud, visualizer_o_Ptr );

	
	// --- Calculate Scene Normals --- //

	pcl::PointCloud<pcl::Normal>::Ptr pSceneNormals( new pcl::PointCloud<pcl::Normal>() );
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normEst;
	normEst.setKSearch(10);
	normEst.setInputCloud( cloud );
	normEst.compute( *pSceneNormals );


	// --- Get Rid Of Floor --- //

	pcl::PointIndices::Ptr inliers_plane( new pcl::PointIndices );
	pcl::ModelCoefficients::Ptr coefficients_plane( new pcl::ModelCoefficients );

	pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg1; 
	seg1.setOptimizeCoefficients( true );
	seg1.setModelType( pcl::SACMODEL_NORMAL_PLANE );
	seg1.setNormalDistanceWeight( 0.05 );
	seg1.setMethodType( pcl::SAC_RANSAC );
	seg1.setMaxIterations( 100 );
	seg1.setDistanceThreshold( 0.075 );
	seg1.setInputCloud( cloud );
	seg1.setInputNormals( pSceneNormals );
	// Obtain the plane inliers and coefficients
	seg1.segment( *inliers_plane, *coefficients_plane );

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

	// Write the planar inliers to disk
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane( new pcl::PointCloud<pcl::PointXYZ> );
	extract.filter( *cloud_plane );

	// Remove the planar inliers, extract the rest
	pcl::PointCloud<pcl::PointXYZ>::Ptr filteredScene( new pcl::PointCloud<pcl::PointXYZ> );
	extract.setNegative( true );
	extract.filter( *filteredScene );

	pcl::ExtractIndices<pcl::Normal> extract_normals;
	pcl::PointCloud<pcl::Normal>::Ptr filteredSceneNormals( new pcl::PointCloud<pcl::Normal> );
	extract_normals.setNegative( true );
	extract_normals.setInputCloud( pSceneNormals );
	extract_normals.setIndices( inliers_plane );
	extract_normals.filter( *filteredSceneNormals );	

	if( filteredScene->points.size() == 0 )
		return;
	
	
	// --- Set Our Target Cloud --- //

	// Assign to the target FeatureCloud
	FeatureCloud target_cloud;
	target_cloud.setInputCloud( filteredScene );


	// --- Visualize the Filtered Cloud --- //

	//visualize( filteredScene, visualizer_o_Ptr );


	// --- Set Input Cloud For Alignment --- //
	
	template_align.setTargetCloud( target_cloud );
	

	// --- Align Templates --- //

	std::cout << "Searching for best fit" << std::endl;
	// Find the best template alignment
	TemplateAlignment::Result best_alignment;
	int best_index = template_align.findBestAlignment( best_alignment );
	std::cerr << "Best alignment index:  " << best_index << std::endl;
	const FeatureCloud &best_template = object_templates[best_index];


	// --- Report Best Match --- //
	
	// Print the alignment fitness score (values less than 0.00002 are good)
	std::cerr << "Best fitness score: " << best_alignment.fitness_score << std::endl;

	// Print the rotation matrix and translation vector
	Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);
	Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3);

	std::cerr << std::setprecision(3);
	std::cerr << std::endl;
	std::cerr << "    | " << rotation(0,0) << " " << rotation(0,1) << " " << rotation(0,2) << " | " << std::endl;
	std::cerr << "R = | " << rotation(1,0) << " " << rotation(1,1) << " " << rotation(1,2) << " | " << std::endl;
	std::cerr << "    | " << rotation(2,0) << " " << rotation(2,1) << " " << rotation(2,2) << " | " << std::endl;
	std::cerr << std::endl;
	std::cerr << "t = < " << translation(0) << ", " << translation(1) << ", " << translation(2) << " >" << std::endl << std::endl;


	// pcl::PointCloud<pcl::PointXYZ> transformedCloud;
	// pcl::transformPointCloud( *best_template.getPointCloud(), transformedCloud, best_alignment.final_transformation);
	// visualize( filteredScene, transformedCloud.makeShared(), visualizer_o_Ptr );
	

	// --- Publish --- //

	// TODO:  Clean up this part.
    geometry_msgs::Pose pose;

	tf::Matrix3x3 rot( rotation(0,0), rotation(0,1), rotation(0,2),
					   rotation(1,0), rotation(1,1), rotation(1,2),
					   rotation(2,0), rotation(2,1), rotation(2,2) );
	tf::Quaternion q;
	rot.getRotation(q);
	pose.orientation.w = q.getW();
	pose.orientation.x = q.getX();
	pose.orientation.y = q.getY();
	pose.orientation.z = q.getZ();

	tf::Vector3 t( translation(0), translation(1), translation(2) );
	pose.position.x = t.getX();
	pose.position.y = t.getY();
	pose.position.z = t.getZ();
	
	std::cerr << "Publishing" << std::endl;
	pub.publish(pose);

	sensor_msgs::PointCloud2 toPub;
    pcl::toROSMsg( *filteredScene, toPub );
	cloud_pub.publish(toPub);
}
Exemplo n.º 3
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);

}
Exemplo 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);
}
Exemplo n.º 5
0
void callback(const sensor_msgs::PointCloud2::ConstPtr& 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::PointXYZ> extract_indices2;

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

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

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

    // The point clouds
    sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr plane_seg_output_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr sphere_RANSAC_output_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr rest_whole_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr whole_pc (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr ball_candidate_output_cloud (new sensor_msgs::PointCloud2);

    // The PointCloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_output (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr remove_plane_cloud (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>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_RANSAC_output (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr remove_false_ball_candidate (new pcl::PointCloud<pcl::PointXYZ>);

    std::vector<int> inliers;

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

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


    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /*
     * Pass through Filtering
     */
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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

    pass.setInputCloud (cloud);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0, 2.5);
    pass.filter (*passthrough_filtered);

    passthrough_pub.publish(passthrough_filtered);

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

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /*
     * for plane features pcl::SACSegmentation
     */
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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

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

    // Optional
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setAxis(Eigen::Vector3f (0, -1, 0));       // best plane should be perpendicular to z-axis
    seg.setMaxIterations (40);
    seg.setDistanceThreshold (0.05);
    seg.setInputCloud (plane_seg_cloud);
    seg.segment (*inliers_plane, *coefficients_plane);

    extract_indices.setInputCloud(plane_seg_cloud);
    extract_indices.setIndices(inliers_plane);
    extract_indices.setNegative(false);
    extract_indices.filter(*plane_seg_output);


    pcl::toROSMsg (*plane_seg_output, *plane_seg_output_cloud);
    plane_pub.publish(plane_seg_output_cloud);

    ros::Time plane_seg_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_plane_cloud);
    plane_seg_cloud.swap (remove_plane_cloud);

    // publish result of Removal the planar inliers, extract the rest
    pcl::toROSMsg (*plane_seg_cloud, *rest_whole_cloud);
    rest_whole_pub.publish(rest_whole_cloud);          // 'rest_whole_cloud' substituted whole_pc

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

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

    ros::Time find_ball_start = ros::Time::now();
    int iter = 0;

    while(iter < 5)
    {
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /*
         * for sphere features pcl::SACSegmentation
         */
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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

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

        // Optional
        seg.setOptimizeCoefficients (false);
        // Mandatory
        seg.setModelType (pcl::SACMODEL_SPHERE);
        seg.setMethodType (pcl::SAC_RANSAC);
        seg.setMaxIterations (10000);
        seg.setDistanceThreshold (0.03);
        seg.setRadiusLimits (0.12, 0.16);
        seg.setInputCloud (sphere_cloud);
        seg.segment (*inliers_sphere, *coefficients_sphere);
        //std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;


        if (inliers_sphere->indices.empty())
            std::cerr << "[[--Can't find the Sphere Candidate.--]]" << std::endl;
        else {
            extract_indices.setInputCloud(sphere_cloud);
            extract_indices.setIndices(inliers_sphere);
            extract_indices.setNegative(false);
            extract_indices.filter(*sphere_output);
            pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
            sphere_seg_pub.publish(sphere_output_cloud);          // 'sphere_output_cloud' means ball candidate point cloud
        }

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

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


        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /*
         * for sphere features pcl::SampleConsensusModelSphere
         */
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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

        // created RandomSampleConsensus object and compute the appropriated model
        pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (sphere_output));

        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
        ransac.setDistanceThreshold (.01);
        ransac.computeModel();
        ransac.getInliers(inliers);

        // copies all inliers of the model computed to another PointCloud
        pcl::copyPointCloud<pcl::PointXYZ>(*sphere_output, inliers, *sphere_RANSAC_output);

        pcl::toROSMsg (*sphere_RANSAC_output, *sphere_RANSAC_output_cloud);

        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /*
         * To discriminate a ball
         */
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        double w = 0;
        w = double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
            /double(sphere_output_cloud->width * sphere_output_cloud->height);

        if (w > 0.9) {
            BALL = true;
            std::cout << "can find a ball" << std::endl;
            true_ball_pub.publish(sphere_RANSAC_output_cloud);
            break;
        } else {
            BALL = false;
            std::cout << "can not find a ball" << std::endl;

            //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
            /*
             * Exclude false ball candidate
             */
            //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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

            // Create the filtering object
            // Remove the planar inliers, extract the rest
            extract_indices2.setInputCloud(plane_seg_cloud);
            extract_indices2.setIndices(inliers_sphere);
            extract_indices2.setNegative (true);
            extract_indices2.filter (*remove_false_ball_candidate);
            sphere_RANSAC_output.swap (remove_false_ball_candidate);

            // publish result of Removal the planar inliers, extract the rest
            pcl::toROSMsg (*sphere_RANSAC_output, *ball_candidate_output_cloud);
            rest_ball_candidate_pub.publish(ball_candidate_output_cloud);
            rest_whole_cloud = ball_candidate_output_cloud;

        }
        iter++;
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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

    }
    ros::Time find_ball_end = ros::Time::now();


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

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

    std::cout << "cloud size             : " << cloud->width * cloud->height << std::endl;
    std::cout << "plane size             : " << plane_seg_output_cloud->width * plane_seg_output_cloud->height << std::endl;
    std::cout << "rest size              : " << rest_whole_cloud->width * rest_whole_cloud->height << std::endl;
    std::cout << "sphere size            : " << sphere_output_cloud->width * sphere_output_cloud->height << std::endl;
    std::cout << "sphere RANSAC size     : " << sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height << "   " << sphere_RANSAC_output->points.size() << std::endl;
    std::cout << "sphereness             : " << double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
              /double(sphere_output_cloud->width * sphere_output_cloud->height) << std::endl;

    std::cout << "model coefficient      : " << coefficients_plane->values[0] << " "
              << coefficients_plane->values[1] << " "
              << coefficients_plane->values[2] << " "
              << coefficients_plane->values[3] << " " << std::endl;



    printf("\n");

    std::cout << "whole time             : " << whole_end - whole_start << " sec" << std::endl;
    std::cout << "declare types time     : " << declare_types_end - declare_types_start << " sec" << std::endl;
    std::cout << "passthrough time       : " << pass_end - pass_start << " sec" << std::endl;
    std::cout << "plane time             : " << plane_seg_end - plane_seg_start << " sec" << std::endl;
    std::cout << "rest and pass time     : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
    std::cout << "find a ball time       : " << find_ball_end - find_ball_start << " sec" << std::endl;
    //std::cout << "sphere time            : " << sphere_end - sphere_start << " sec" << std::endl;
    //std::cout << "sphere ransac time     : " << sphere_RANSAC_end - sphere_RANSAC_start << " sec" << std::endl;
    printf("\n----------------------------------------------------------------------------\n");
    printf("\n");
}
Exemplo n.º 6
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);

}