bool EuclideanClustering::serviceCallback( jsk_recognition_msgs::EuclideanSegment::Request &req, jsk_recognition_msgs::EuclideanSegment::Response &res) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(req.input, *cloud); #if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 ) pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > (); #else pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>); tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > (); #endif vector<pcl::PointIndices> cluster_indices; EuclideanClusterExtraction<pcl::PointXYZ> impl; double tor; if ( req.tolerance < 0) { tor = tolerance; } else { tor = req.tolerance; } impl.setClusterTolerance (tor); impl.setMinClusterSize (minsize_); impl.setMaxClusterSize (maxsize_); impl.setSearchMethod (tree); impl.setInputCloud (cloud); impl.extract (cluster_indices); res.output.resize( cluster_indices.size() ); #if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 ) pcl::PCLPointCloud2::Ptr pcl_cloud(new pcl::PCLPointCloud2); pcl_conversions::toPCL(req.input, *pcl_cloud); pcl::ExtractIndices<pcl::PCLPointCloud2> ex; ex.setInputCloud(pcl_cloud); #else pcl::ExtractIndices<sensor_msgs::PointCloud2> ex; ex.setInputCloud ( boost::make_shared< sensor_msgs::PointCloud2 > (req.input) ); #endif for ( size_t i = 0; i < cluster_indices.size(); i++ ) { ex.setIndices ( boost::make_shared< pcl::PointIndices > (cluster_indices[i]) ); ex.setNegative ( false ); #if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 ) pcl::PCLPointCloud2 output_cloud; ex.filter ( output_cloud ); pcl_conversions::fromPCL(output_cloud, res.output[i]); #else ex.filter ( res.output[i] ); #endif } return true; }
static void clustering(PointCloudPtr cloudPCLInput, vector<PointIndices> &vecSegmentIndices, double dClusterTolerance, unsigned int unMinClusterSize, unsigned int unMaxClusterSize) { vecSegmentIndices.clear(); typedef pcl::search::KdTree<PointT> KdTree; typedef typename KdTree::Ptr KdTreePtr; EuclideanClusterExtraction<PointT> cluster; KdTreePtr cluster_tree = boost::make_shared<pcl::search::KdTree<PointT> > (); cluster.setInputCloud(cloudPCLInput); cluster.setClusterTolerance(dClusterTolerance); cluster.setMinClusterSize(unMinClusterSize); cluster.setMaxClusterSize(unMaxClusterSize); cluster.setSearchMethod(cluster_tree); cluster.extract(vecSegmentIndices); }
void EuclideanClustering::extract( const sensor_msgs::PointCloud2ConstPtr &input) { boost::mutex::scoped_lock lock(mutex_); vital_checker_->poke(); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*input, *cloud); std::vector<pcl::PointIndices> cluster_indices; // list up indices which are not NaN to use // organized pointcloud pcl::PointIndices::Ptr nonnan_indices (new pcl::PointIndices); for (size_t i = 0; i < cloud->points.size(); i++) { pcl::PointXYZ p = cloud->points[i]; if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) { nonnan_indices->indices.push_back(i); } } if (nonnan_indices->indices.size() == 0) { // if input points is 0, publish empty data as result jsk_recognition_msgs::ClusterPointIndices result; result.header = input->header; result_pub_.publish(result); // do nothing and return it jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped); cluster_num_msg->header = input->header; cluster_num_msg->data = 0; cluster_num_pub_.publish(cluster_num_msg); return; } EuclideanClusterExtraction<pcl::PointXYZ> impl; { jsk_topic_tools::ScopedTimer timer = kdtree_acc_.scopedTimer(); #if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 ) pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > (); #else pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>); tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > (); #endif tree->setInputCloud (cloud); impl.setClusterTolerance (tolerance); impl.setMinClusterSize (minsize_); impl.setMaxClusterSize (maxsize_); impl.setSearchMethod (tree); impl.setIndices(nonnan_indices); impl.setInputCloud (cloud); } { jsk_topic_tools::ScopedTimer timer = segmentation_acc_.scopedTimer(); impl.extract (cluster_indices); } // Publish result indices jsk_recognition_msgs::ClusterPointIndices result; result.cluster_indices.resize(cluster_indices.size()); cluster_counter_.add(cluster_indices.size()); result.header = input->header; if (cogs_.size() != 0 && cogs_.size() == cluster_indices.size()) { // tracking the labels //ROS_INFO("computing distance matrix"); // compute distance matrix // D[i][j] --> distance between the i-th previous cluster // and the current j-th cluster Vector4fVector new_cogs; computeCentroidsOfClusters(new_cogs, cloud, cluster_indices); double D[cogs_.size() * new_cogs.size()]; computeDistanceMatrix(D, cogs_, new_cogs); std::vector<int> pivot_table = buildLabelTrackingPivotTable(D, cogs_, new_cogs, label_tracking_tolerance); if (pivot_table.size() != 0) { cluster_indices = pivotClusterIndices(pivot_table, cluster_indices); } } Vector4fVector tmp_cogs; computeCentroidsOfClusters(tmp_cogs, cloud, cluster_indices); // NB: not efficient cogs_ = tmp_cogs; for (size_t i = 0; i < cluster_indices.size(); i++) { #if ROS_VERSION_MINIMUM(1, 10, 0) // hydro and later result.cluster_indices[i].header = pcl_conversions::fromPCL(cluster_indices[i].header); #else // groovy result.cluster_indices[i].header = cluster_indices[i].header; #endif result.cluster_indices[i].indices = cluster_indices[i].indices; } result_pub_.publish(result); jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped); cluster_num_msg->header = input->header; cluster_num_msg->data = cluster_indices.size(); cluster_num_pub_.publish(cluster_num_msg); diagnostic_updater_->update(); }
// call Euclidean Cluster Extraction (ref: http://www.pointclouds.org/documentation/tutorials/cluster_extraction.php) bool clusterize(ClusterSegmentation::Request &req, ClusterSegmentation::Response &res){ // get data and convert in useful format considering also default PCLCloudPtr cloud = pcm::PCManager::cloudForRosMsg( req.cloud); double tolerance, minClusterSizeRate, maxClusterSizeRate; int minInputSize; nh_ptr->param(srvm::PARAM_NAME_CLUSTER_TOLERANCE, tolerance, CLUSTER_TOLERANCE_DEFAULT); nh_ptr->param(srvm::PARAM_NAME_CLUSTER_MIN_RATE, minClusterSizeRate, CLUSTER_MIN_RATE_DEFAULT); nh_ptr->param(srvm::PARAM_NAME_CLUSTER_MAX_RATE, maxClusterSizeRate, CLUSTER_MAX_RATE_DEFAULT); nh_ptr->param(srvm::PARAM_NAME_CLUSTER_TOLERANCE, minInputSize, CLUSTER_MIN_INPUT_SIZE_DEFAULT); if( cloud->points.size() >= minInputSize){ // skip if input cloud is too small // Creating the KdTree object for the search method of the extraction search::KdTree< PointXYZ>::Ptr tree (new search::KdTree< PointXYZ>); tree->setInputCloud ( cloud); // compute clusters vector< PointIndices> cluster_indices; EuclideanClusterExtraction< PointXYZ> ec; ec.setClusterTolerance(tolerance); // in meters ec.setMinClusterSize( round(cloud->points.size () * minClusterSizeRate)); // percentage ec.setMaxClusterSize( round(cloud->points.size () * maxClusterSizeRate)); //ROS_ERROR( "%d %f %f", cloud->points.size(), cloud->points.size () * minClusterSizeRate, cloud->points.size () * maxClusterSizeRate); ec.setSearchMethod( tree); ec.setInputCloud( cloud); ec.extract( cluster_indices); // for all the cluster for ( vector< pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ // build inliers output vector< int>* inlier (new std::vector< int>); PCLCloudPtr cloudExtraxted (new PCLCloud); // build centroid output float xSumm = 0, ySumm = 0, zSumm = 0; int cnt = 1; // for all the point of a cluster (create a new point cloud for every clusters) for ( vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){ // set inlier inlier->push_back( *pit); // set a point of the cloud PointXYZ* p ( new PointXYZ( cloud->points[*pit].x, cloud->points[*pit].y, cloud->points[*pit].z)); cloudExtraxted->push_back( *p); // save summ to compute avarage xSumm += cloud->points[*pit].x; ySumm += cloud->points[*pit].y; zSumm += cloud->points[*pit].z; cnt++; } // create returning object InliersCluster* cluster ( new InliersCluster); cluster->inliers = *inlier; cluster->cloud = PCManager::cloudToRosMsg( cloudExtraxted); // compute and assign to output the cluster centroid cluster->x_centroid = xSumm / cnt; cluster->y_centroid = ySumm / cnt; cluster->z_centroid = zSumm / cnt; // add to returning values res.cluster_objs.push_back( *cluster); } } return true; }
int main(int argc, char** argv) { if (argc < 2) { cout << "Input a PCD file name...\n"; return 0; } PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>), cloud_f(new PointCloud<PointXYZ>); PCDReader reader; reader.read(argv[1], *cloud); cout << "PointCloud before filtering has: " << cloud->points.size() << " data points.\n"; PointCloud<PointXYZ>::Ptr cloud_filtered(new PointCloud<PointXYZ>); VoxelGrid<PointXYZ> vg; vg.setInputCloud(cloud); vg.setLeafSize(0.01f, 0.01f, 0.01f); vg.filter(*cloud_filtered); cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points.\n"; SACSegmentation<PointXYZ> seg; PointIndices::Ptr inliers(new PointIndices); PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ>); ModelCoefficients::Ptr coefficients(new ModelCoefficients); seg.setOptimizeCoefficients(true); seg.setModelType(SACMODEL_PLANE); seg.setMethodType(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) { seg.setInputCloud(cloud_filtered); seg.segment(*inliers, *coefficients); if (inliers->indices.size() == 0) { cout << "Coud not estimate a planar model for the given dataset.\n"; break; } ExtractIndices<PointXYZ> extract; extract.setInputCloud(cloud_filtered); extract.setIndices(inliers); extract.setNegative(false); extract.filter(*cloud_plane); cout << "PointCloud representing the planar component has: " << cloud_filtered->points.size() << " data points.\n"; extract.setNegative(true); extract.filter(*cloud_f); cloud_filtered->swap(*cloud_f); } search::KdTree<PointXYZ>::Ptr kdtree(new search::KdTree<PointXYZ>); kdtree->setInputCloud(cloud_filtered); vector<PointIndices> cluster_indices; EuclideanClusterExtraction<PointXYZ> ece; ece.setClusterTolerance(0.02); ece.setMinClusterSize(100); ece.setMaxClusterSize(25000); ece.setSearchMethod(kdtree); ece.setInputCloud(cloud_filtered); ece.extract(cluster_indices); PCDWriter writer; int j = 0; for (vector<PointIndices>::const_iterator it=cluster_indices.begin(); it != cluster_indices.end(); ++it) { PointCloud<PointXYZ>::Ptr cluster_cloud(new PointCloud<PointXYZ>); for (vector<int>::const_iterator pit=it->indices.begin(); pit != it->indices.end(); ++pit) cluster_cloud->points.push_back(cloud_filtered->points[*pit]); cluster_cloud->width = cluster_cloud->points.size(); cluster_cloud->height = 1; cluster_cloud->is_dense = true; cout << "PointCloud representing a cluster has: " << cluster_cloud->points.size() << " data points.\n"; stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; writer.write<PointXYZ>(ss.str(), *cluster_cloud, false); j++; } return 0; }