bool segmentCloud (pointing_object_evaluation::ObjectSegmentationRequest::Request &req, pointing_object_evaluation::ObjectSegmentationRequest::Response &res) { CloudPtr cloud (new Cloud ()); { //boost::lock_guard<boost::mutex> lock (cloud_mutex_); *cloud = *cloud_; } unsigned char red [6] = {255, 0, 0, 255, 255, 0}; unsigned char grn [6] = { 0, 255, 0, 255, 0, 255}; unsigned char blu [6] = { 0, 0, 255, 0, 255, 255}; pcl::PointCloud<pcl::PointXYZRGB> aggregate_cloud; // Estimate Normals double ne_start = pcl::getTime (); pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); ne_.setInputCloud (cloud); ne_.compute (*normal_cloud); double ne_end = pcl::getTime (); //std::cout << "NE took: " << double(ne_end - ne_start) << std::endl; // Segment Planes double mps_start = pcl::getTime (); std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; std::vector<pcl::ModelCoefficients> model_coefficients; std::vector<pcl::PointIndices> inlier_indices; pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>); std::vector<pcl::PointIndices> label_indices; std::vector<pcl::PointIndices> boundary_indices; mps_.setInputNormals (normal_cloud); mps_.setInputCloud (cloud); //mps.segment (regions); mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices); //Segment Objects typename pcl::PointCloud<PointT>::CloudVectorType clusters; if (regions.size () > 0) { //printf ("got some planes!\n"); std::vector<bool> plane_labels; plane_labels.resize (label_indices.size (), false); for (size_t i = 0; i < label_indices.size (); i++) { if (label_indices[i].indices.size () > 10000) { plane_labels[i] = true; } } //printf ("made label vec\n"); typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>()); euclidean_cluster_comparator_->setInputCloud (cloud); euclidean_cluster_comparator_->setLabels (labels); euclidean_cluster_comparator_->setExcludeLabels (plane_labels); euclidean_cluster_comparator_->setDistanceThreshold (0.01f, false); pcl::PointCloud<pcl::Label> euclidean_labels; std::vector<pcl::PointIndices> euclidean_label_indices; pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_); euclidean_segmentation.setInputCloud (cloud); euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices); //printf ("done segmenting the clusters\n"); std::vector<Eigen::Vector4f> centroids; for (size_t i = 0; i < euclidean_label_indices.size (); i++) { if (euclidean_label_indices[i].indices.size () > 1000) { pcl::PointCloud<PointT> cluster; pcl::copyPointCloud (*cloud,euclidean_label_indices[i].indices,cluster); clusters.push_back (cluster); /*Eigen::Vector4f centroid; pcl::compute3DCentroid (cluster, centroid); centroids.push_back (centroid);*/ //pcl::PointXYZ centroid_pt (centroid[0], centroid[1], centroid[2]); //double dist = sqrt (centroid[0]*centroid[0] + centroid[1]*centroid[1] + centroid[2]*centroid[2]); //object_points_.push_back (centroid_pt); // Send to RViz pcl::PointCloud<pcl::PointXYZRGB> color_cluster; pcl::copyPointCloud (cluster, color_cluster); for (size_t j = 0; j < color_cluster.size (); j++) { color_cluster.points[j].r = (color_cluster.points[j].r + red[i%6]) / 2; color_cluster.points[j].g = (color_cluster.points[j].g + grn[i%6]) / 2; color_cluster.points[j].b = (color_cluster.points[j].b + blu[i%6]) / 2; } aggregate_cloud += color_cluster; } } PCL_INFO ("Got %d euclidean clusters!\n", clusters.size ()); //PCL_INFO ("Got %d centroids!\n", centroids.size ()); aggregate_cloud.header = cloud->header; sensor_msgs::PointCloud2 cloud_msg; pcl::toROSMsg (aggregate_cloud, cloud_msg); object_cloud_pub_.publish (cloud_msg); // TODO: mutex /*clusters_ = clusters; centroids_= centroids;*/ } }
void MultiplaneSegmenter<PointT>::segment() { clusters_.clear(); computeTablePlanes(); typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator< PointT, pcl::Normal,pcl::Label> ()); pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>); labels->points.resize( scene_->points.size() ); std::vector < pcl::PointIndices > label_indices(2); std::vector<bool> plane_labels; plane_labels.resize (label_indices.size (), false); if ( !all_planes_.empty() ) { size_t table_plane_selected = 0; size_t max_inliers_found = 0; std::vector<size_t> plane_inliers_counts (all_planes_.size (), 0); for (size_t i = 0; i < all_planes_.size (); i++) { const Eigen::Vector3f plane_normal = all_planes_[i]->coefficients_.head(3); for (size_t j = 0; j < scene_->points.size (); j++) { if ( !pcl::isFinite( scene_->points[j] ) ) continue; const Eigen::Vector3f &xyz_p = scene_->points[j].getVector3fMap (); if (std::abs ( xyz_p.dot(plane_normal) ) < param_.sensor_noise_max_) plane_inliers_counts[i]++; } if ( plane_inliers_counts[i] > max_inliers_found ) { table_plane_selected = i; max_inliers_found = plane_inliers_counts[i]; } } size_t itt = table_plane_selected; dominant_plane_ = all_planes_[itt]->coefficients_; Eigen::Vector3f normal_table = all_planes_[itt]->coefficients_.head(3); size_t inliers_count_best = plane_inliers_counts[itt]; //check that the other planes with similar normal are not higher than the table_plane_selected for (size_t i = 0; i < all_planes_.size (); i++) { Eigen::Vector4f model = all_planes_[i]->coefficients_; Eigen::Vector3f normal_tmp = all_planes_[i]->coefficients_.head(3); int inliers_count = plane_inliers_counts[i]; if ((normal_tmp.dot (normal_table) > 0.95) && (inliers_count_best * 0.5 <= inliers_count)) { //check if this plane is higher, projecting a point on the normal direction std::cout << model[3] << " " << dominant_plane_[3] << std::endl; if (model[3] < dominant_plane_[3]) { PCL_WARN ("Changing table plane..."); dominant_plane_ = all_planes_[i]->coefficients_; normal_table = normal_tmp; inliers_count_best = inliers_count; } } } dominant_plane_ = all_planes_[table_plane_selected]->coefficients_; //create two labels, 1 one for points belonging to or under the plane, 1 for points above the plane for (size_t j = 0; j < scene_->points.size (); j++) { const Eigen::Vector3f &xyz_p = scene_->points[j].getVector3fMap (); if (! pcl::isFinite(scene_->points[j]) ) continue; float val = xyz_p[0] * dominant_plane_[0] + xyz_p[1] * dominant_plane_[1] + xyz_p[2] * dominant_plane_[2] + dominant_plane_[3]; if (val >= param_.sensor_noise_max_) { labels->points[j].label = 1; label_indices[0].indices.push_back (j); } else { labels->points[j].label = 0; label_indices[1].indices.push_back (j); } } plane_labels[0] = true; } else { for (size_t j = 0; j < scene_->points.size (); j++) labels->points[j].label = 1; } euclidean_cluster_comparator_->setInputCloud (scene_); euclidean_cluster_comparator_->setLabels (labels); euclidean_cluster_comparator_->setExcludeLabels (plane_labels); euclidean_cluster_comparator_->setDistanceThreshold ( param_.min_distance_between_clusters_, true); pcl::PointCloud < pcl::Label > euclidean_labels; std::vector < pcl::PointIndices > euclidean_label_indices; pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_); euclidean_segmentation.setInputCloud (scene_); euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices); for (size_t i = 0; i < euclidean_label_indices.size (); i++) { if ( euclidean_label_indices[i].indices.size () >= param_.min_cluster_size_) { clusters_.push_back (euclidean_label_indices[i]); } } }
void OrganizedMultiplaneSegmenter<PointT>::segment() { clusters_.clear(); pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (param_.num_plane_inliers_); mps.setAngularThreshold (param_.angular_threshold_deg_ * M_PI/180.f); mps.setDistanceThreshold (param_.sensor_noise_max_); mps.setInputNormals (normals_); mps.setInputCloud (scene_); std::vector < pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; std::vector < pcl::ModelCoefficients > model_coeff; std::vector < pcl::PointIndices > inlier_indices; pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>); std::vector < pcl::PointIndices > label_indices; std::vector < pcl::PointIndices > boundary_indices; typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp ( new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ()); ref_comp->setDistanceThreshold (param_.sensor_noise_max_, false); ref_comp->setAngularThreshold (2 * M_PI/180.f); mps.setRefinementComparator (ref_comp); mps.segmentAndRefine (regions, model_coeff, inlier_indices, labels, label_indices, boundary_indices); std::cout << "Number of planes found:" << model_coeff.size () << std::endl; if ( !model_coeff.size() ) return; size_t table_plane_selected = 0; int max_inliers_found = -1; std::vector<size_t> plane_inliers_counts; plane_inliers_counts.resize (model_coeff.size ()); for (size_t i = 0; i < model_coeff.size (); i++) { Eigen::Vector4f table_plane = Eigen::Vector4f (model_coeff[i].values[0], model_coeff[i].values[1], model_coeff[i].values[2], model_coeff[i].values[3]); std::cout << "Number of inliers for this plane:" << inlier_indices[i].indices.size () << std::endl; size_t remaining_points = 0; typename pcl::PointCloud<PointT>::Ptr plane_points (new pcl::PointCloud<PointT> (*scene_)); for (size_t j = 0; j < plane_points->points.size (); j++) { const Eigen::Vector3f xyz_p = plane_points->points[j].getVector3fMap (); if ( !pcl::isFinite( plane_points->points[j] ) ) continue; float val = xyz_p[0] * table_plane[0] + xyz_p[1] * table_plane[1] + xyz_p[2] * table_plane[2] + table_plane[3]; if (std::abs (val) > param_.sensor_noise_max_) { plane_points->points[j].x = std::numeric_limits<float>::quiet_NaN (); plane_points->points[j].y = std::numeric_limits<float>::quiet_NaN (); plane_points->points[j].z = std::numeric_limits<float>::quiet_NaN (); } else remaining_points++; } plane_inliers_counts[i] = remaining_points; if ( (int)remaining_points > max_inliers_found ) { table_plane_selected = i; max_inliers_found = remaining_points; } } size_t itt = table_plane_selected; dominant_plane_ = Eigen::Vector4f (model_coeff[itt].values[0], model_coeff[itt].values[1], model_coeff[itt].values[2], model_coeff[itt].values[3]); Eigen::Vector3f normal_table = Eigen::Vector3f (model_coeff[itt].values[0], model_coeff[itt].values[1], model_coeff[itt].values[2]); size_t inliers_count_best = plane_inliers_counts[itt]; //check that the other planes with similar normal are not higher than the table_plane_selected for (size_t i = 0; i < model_coeff.size (); i++) { Eigen::Vector4f model = Eigen::Vector4f (model_coeff[i].values[0], model_coeff[i].values[1], model_coeff[i].values[2], model_coeff[i].values[3]); Eigen::Vector3f normal = Eigen::Vector3f (model_coeff[i].values[0], model_coeff[i].values[1], model_coeff[i].values[2]); int inliers_count = plane_inliers_counts[i]; std::cout << "Dot product is:" << normal.dot (normal_table) << std::endl; if ((normal.dot (normal_table) > 0.95) && (inliers_count_best * 0.5 <= inliers_count)) { //check if this plane is higher, projecting a point on the normal direction std::cout << "Check if plane is higher, then change table plane" << std::endl; std::cout << model[3] << " " << dominant_plane_[3] << std::endl; if (model[3] < dominant_plane_[3]) { PCL_WARN ("Changing table plane..."); table_plane_selected = i; dominant_plane_ = model; normal_table = normal; inliers_count_best = inliers_count; } } } dominant_plane_ = Eigen::Vector4f (model_coeff[table_plane_selected].values[0], model_coeff[table_plane_selected].values[1], model_coeff[table_plane_selected].values[2], model_coeff[table_plane_selected].values[3]); typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator< PointT, pcl::Normal,pcl::Label> ()); //create two labels, 1 one for points belonging to or under the plane, 1 for points above the plane label_indices.resize (2); for (size_t j = 0; j < scene_->points.size (); j++) { Eigen::Vector3f xyz_p = scene_->points[j].getVector3fMap (); if (! pcl::isFinite(scene_->points[j]) ) continue; float val = xyz_p[0] * dominant_plane_[0] + xyz_p[1] * dominant_plane_[1] + xyz_p[2] * dominant_plane_[2] + dominant_plane_[3]; if (val >= param_.sensor_noise_max_) { /*plane_points->points[j].x = std::numeric_limits<float>::quiet_NaN (); plane_points->points[j].y = std::numeric_limits<float>::quiet_NaN (); plane_points->points[j].z = std::numeric_limits<float>::quiet_NaN ();*/ labels->points[j].label = 1; label_indices[0].indices.push_back (j); } else { labels->points[j].label = 0; label_indices[1].indices.push_back (j); } } std::vector<bool> plane_labels; plane_labels.resize (label_indices.size (), false); plane_labels[0] = true; euclidean_cluster_comparator_->setInputCloud (scene_); euclidean_cluster_comparator_->setLabels (labels); euclidean_cluster_comparator_->setExcludeLabels (plane_labels); euclidean_cluster_comparator_->setDistanceThreshold (0.035f, true); pcl::PointCloud < pcl::Label > euclidean_labels; std::vector < pcl::PointIndices > euclidean_label_indices; pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_); euclidean_segmentation.setInputCloud (scene_); euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices); for (size_t i = 0; i < euclidean_label_indices.size (); i++) { if ( (int)euclidean_label_indices[i].indices.size () >= param_.min_cluster_size_) { clusters_.push_back (euclidean_label_indices[i]); } } }