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"); }
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); }
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; // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; std::vector<int> inliers; // The plane and sphere coefficients pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ()); // The plane and sphere inliers 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 passthrough_filtered (new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2); // The PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>); ros::Time declare_types_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Create Voxel grid 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); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * Pass through Filtering */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ros::Time pass_start = ros::Time::now(); pass.setInputCloud (cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (0, 3.5); pass.filter (*passthrough_filtered); pass.setInputCloud (passthrough_filtered); pass.setFilterFieldName ("y"); pass.setFilterLimits (-0.1, 0.3); pass.filter (*passthrough_filtered); passthrough_pub.publish(passthrough_filtered); ros::Time pass_end = ros::Time::now(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * for sphere features */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud pcl::fromROSMsg (*passthrough_filtered, *sphere_cloud); ros::Time sphere_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_cloud)); 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_cloud, inliers, *sphere_output); pcl::toROSMsg (*sphere_output, *sphere_output_cloud); sphere_pub.publish(sphere_output_cloud); // // Optional // seg.setOptimizeCoefficients (true); // // Mandatory // seg.setModelType (pcl::SACMODEL_SPHERE); // seg.setMethodType (pcl::SAC_RANSAC); // seg.setMaxIterations (10000); // seg.setDistanceThreshold (0.05); // seg.setRadiusLimits (0, 0.15); // seg.setInputCloud (sphere_cloud); // seg.segment (*inliers_sphere, *coefficients_sphere); // //std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl; // // // 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(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /* * visualize normals */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // pcl::visualization::PCLVisualizer viewer("PCL Viewer"); // viewer.setBackgroundColor (0.0, 0.0, 0.5); // viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(sphere_cloud, cloud_normals3); // viewer.spin(); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ros::Time whole_end = ros::Time::now(); std::cout << "cloud size : " << cloud->width * cloud->height << std::endl; std::cout << "pass_th size : " << passthrough_filtered->width * passthrough_filtered->height << std::endl; std::cout << "sphere size : " << sphere_output_cloud->width * sphere_output_cloud->height << 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 << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl; printf("\n----------------------------------------------------------------------------\n"); printf("\n"); }
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"); }