void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) { // ... do data processing sensor_msgs::PointCloud2 output; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*input, *cloud); // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.10); // 2cm ec.setMinClusterSize (20); ec.setMaxClusterSize (2500); ec.setSearchMethod (tree); ec.setInputCloud(cloud); ec.extract (cluster_indices); ROS_INFO("cluster_indices has %d data points.", (int) cluster_indices.size()); ROS_INFO("cloud has %d data points.", (int) cloud->points.size()); visualization_msgs::Marker marker; marker.header = cloud->header; marker.id = 1; marker.type = visualization_msgs::Marker::CUBE_LIST; marker.action = visualization_msgs::Marker::ADD; marker.color.r = 1; marker.color.g = 0; marker.color.b = 0; marker.color.a = 0.7; marker.scale.x = 0.2; marker.scale.y = 0.2; marker.scale.z = 0.2; marker.lifetime = ros::Duration(60.0); Eigen::Vector4f minPoint; Eigen::Vector4f maxPoint; // pcl::getMinMax3D(*cloud, minPoint, maxPoint); for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster->points.push_back (cloud->points[*pit]); std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; // Merge current clusters to whole point cloud *clustered_cloud += *cloud_cluster; // for(size_t j = 0; j < cloud_cluster->points.size() - 1; j++) // { /* geometry_msgs::Point pt1; pt1.x = cloud_cluster->points[j].x; pt1.y = cloud_cluster->points[j].y; pt1.z = cloud_cluster->points[j].z; geometry_msgs::Point pt2; pt2.x = cloud_cluster->points[j+1].x; pt2.y = cloud_cluster->points[j+1].y; pt2.z = cloud_cluster->points[j+1].z; marker.points.push_back(pt1); marker.points.push_back(pt2); */ //Seg for top of prism geometry_msgs::Point pt3; pt3.x = 0.0; pt3.y = 0.0; pt3.z = 0.0; std_msgs::ColorRGBA colors; colors.r = 0.0; colors.g = 0.0; colors.b = 0.0; for(size_t i=0; i<cloud_cluster->points.size(); i++) { pt3.x += cloud_cluster->points[i].x; pt3.y += cloud_cluster->points[i].y; pt3.z += cloud_cluster->points[i].z; } pt3.x /= cloud_cluster->points.size(); pt3.y /= cloud_cluster->points.size(); pt3.z /= cloud_cluster->points.size(); pcl::getMinMax3D(*cloud_cluster, minPoint, maxPoint); marker.scale.y= maxPoint.y(); //marker.scale.x= maxPoint.x(); //marker.scale.z= maxPoint.z(); marker.scale.x= maxPoint.x()-minPoint.x(); colors.r = marker.scale.x; // colors.g = marker.scale.y; //marker.scale.z= maxPoint.z()-minPoint.z(); //pt3.z = maxPoint.z(); //geometry_msgs::Point pt4; //pt4.x = cloud_cluster->points[j+1].x; //pt4.y = cloud_cluster->points[j+1].y; //pt4.z = cloud_cluster->points[j+1].z; //pt4.z = maxPoint.z(); //marker.pose.position.x = pt3.x; //marker.pose.position.y = pt3.y; //marker.pose.position.z = pt3.z; //marker.colors.push_back(colors); marker.points.push_back(pt3); //marker.points.push_back(pt4); //Seg for bottom vertices to top vertices // marker.points.push_back(pt1); //marker.points.push_back(pt3); // } object_pub.publish(marker); } // Publish the data pcl::toROSMsg(*clustered_cloud, output); output.header = cloud->header; // output.header.frame_id="/camera_link"; // output.header.stamp = ros::Time::now(); pub.publish (output); }
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input){ sensor_msgs::PointCloud2::Ptr clusters (new sensor_msgs::PointCloud2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*input, *cloud); pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (10); ec.setMaxClusterSize (2500); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); std::vector<pcl::PointIndices>::const_iterator it; std::vector<int>::const_iterator pit; int j = 0; for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); for(pit = it->indices.begin(); pit != it->indices.end(); pit++) { //push_back: add a point to the end of the existing vector cloud_cluster->points.push_back(cloud_filtered->points[*pit]); /*cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; std::stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* j++;*/ } //Merge current clusters to whole point cloud *clustered_cloud += *cloud_cluster; } pcl::toROSMsg (*clustered_cloud , *clusters); clusters->header.frame_id = "/camera_depth_frame"; clusters->header.stamp=ros::Time::now(); pub.publish (*clusters); //sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2); //pcl::toROSMsg(*cloud_filtered, *output); //pub_plane.publish(*output); }