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