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"); }
// 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); }
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); }
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); }
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"); }
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); }