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]); } } }
int main (int, char**) { Point_set pts; pts.add_normal_map(); bool map_added = false; Size_t_map echo_map; Color_map color_map; boost::tie (echo_map, map_added) = pts.add_property_map<std::size_t> ("echo"); CGAL_assertion (map_added); boost::tie (color_map, map_added) = pts.add_property_map<Classification::RGB_Color> ("color"); CGAL_assertion (map_added); for (std::size_t i = 0; i < 1000; ++ i) { Point_set::iterator it = pts.insert (Point (CGAL::get_default_random().get_double(), CGAL::get_default_random().get_double(), CGAL::get_default_random().get_double()), Vector (CGAL::get_default_random().get_double(), CGAL::get_default_random().get_double(), CGAL::get_default_random().get_double())); echo_map[*it] = std::size_t(CGAL::get_default_random().get_int(0, 4)); color_map[*it] = CGAL::make_array ((unsigned char)(CGAL::get_default_random().get_int(0, 255)), (unsigned char)(CGAL::get_default_random().get_int(0, 255)), (unsigned char)(CGAL::get_default_random().get_int(0, 255))); } Feature_set features; Feature_generator generator (features, pts, pts.point_map(), 5, // using 5 scales pts.normal_map(), color_map, echo_map); CGAL_assertion (generator.number_of_scales() == 5); CGAL_assertion (features.size() == 80); Label_set labels; std::vector<int> training_set (pts.size(), -1); for (std::size_t i = 0; i < 20; ++ i) { std::ostringstream oss; oss << "label_" << i; Label_handle lh = labels.add(oss.str().c_str()); for (std::size_t j = 0; j < 10; ++ j) training_set[std::size_t(CGAL::get_default_random().get_int(0, int(training_set.size())))] = int(i); } CGAL_assertion (labels.size() == 20); Classifier classifier (labels, features); classifier.train<CGAL::Sequential_tag> (training_set, 800); #ifdef CGAL_LINKED_WITH_TBB classifier.train<CGAL::Parallel_tag> (training_set, 800); #endif std::vector<int> label_indices(pts.size(), -1); Classification::classify<CGAL::Sequential_tag> (pts, labels, classifier, label_indices); Classification::classify_with_local_smoothing<CGAL::Sequential_tag> (pts, pts.point_map(), labels, classifier, generator.neighborhood().sphere_neighbor_query(0.01f), label_indices); Classification::classify_with_graphcut<CGAL::Sequential_tag> (pts, pts.point_map(), labels, classifier, generator.neighborhood().k_neighbor_query(12), 0.2f, 10, label_indices); #ifdef CGAL_LINKED_WITH_TBB Classification::classify<CGAL::Sequential_tag> (pts, labels, classifier, label_indices); Classification::classify_with_local_smoothing<CGAL::Sequential_tag> (pts, pts.point_map(), labels, classifier, generator.neighborhood().sphere_neighbor_query(0.01f), label_indices); Classification::classify_with_graphcut<CGAL::Sequential_tag> (pts, pts.point_map(), labels, classifier, generator.neighborhood().k_neighbor_query(12), 0.2f, 10, label_indices); #endif Classification::Evaluation evaluation (labels, training_set, label_indices); return EXIT_SUCCESS; }