void OrganizedMultiPlaneExtractor<PointT>::compute() { CHECK ( cloud_ && cloud_->isOrganized() ) << "Input cloud is not organized!"; CHECK ( normal_cloud_ && (normal_cloud_->points.size() == cloud_->points.size() )); pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers ( param_.min_num_plane_inliers_ ); mps.setAngularThreshold ( param_.angular_threshold_deg_ * M_PI/180.f ); mps.setDistanceThreshold ( param_.distance_threshold_); mps.setInputNormals ( normal_cloud_ ); mps.setInputCloud ( cloud_ ); std::vector < pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp ( new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ()); ref_comp->setDistanceThreshold (param_.distance_threshold_, false ); ref_comp->setAngularThreshold ( param_.angular_threshold_deg_ * M_PI/180.f ); mps.setRefinementComparator ( ref_comp ); mps.segmentAndRefine ( regions ); all_planes_.clear(); all_planes_.reserve (regions.size ()); for (const pcl::PlanarRegion<PointT> ® : regions ) { Eigen::Vector4f plane = reg.getCoefficients(); // flip plane normal towards viewpoint Eigen::Vector3f z = Eigen::Vector3f::UnitZ(); if( z.dot(plane.head(3)) > 0 ) plane = -plane; all_planes_.push_back( plane ); } }
void MultiplaneSegmenter<PointT>::computeTablePlanes() { all_planes_.clear(); pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (param_.num_plane_inliers_); mps.setAngularThreshold ( pcl::deg2rad(param_.angular_threshold_deg_) ); 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 ( pcl::deg2rad(2.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; all_planes_.resize ( model_coeff.size() ); for (size_t i = 0; i < model_coeff.size (); i++) { // flip normal of plane towards viewpoint Eigen::Vector3f vp; vp(0)=vp(1)=0.f; vp(2) = 1; Eigen::Vector4f plane_tmp = Eigen::Vector4f(model_coeff[i].values[0], model_coeff[i].values[1], model_coeff[i].values[2], model_coeff[i].values[3]); Eigen::Vector3f table_vec = plane_tmp.head(3); if(vp.dot(table_vec)>0) plane_tmp *= -1.f; all_planes_[i].reset( new PlaneModel<PointT>); all_planes_[i]->coefficients_ = plane_tmp; all_planes_[i]->inliers_ = inlier_indices[i].indices; all_planes_[i]->cloud_ = scene_; typename pcl::PointCloud<PointT>::Ptr plane_cloud (new pcl::PointCloud<PointT>); typename pcl::PointCloud<PointT>::Ptr above_plane_cloud (new pcl::PointCloud<PointT>); pcl::copyPointCloud(*scene_, inlier_indices[i], *plane_cloud); double z_min = 0., z_max = 0.30; // we want the points above the plane, no farther than zmax cm from the surface typename pcl::PointCloud<PointT>::Ptr hull_points = all_planes_[i]->getConvexHullCloud(); pcl::PointIndices cloud_indices; pcl::ExtractPolygonalPrismData<PointT> prism; prism.setInputCloud (scene_); prism.setInputPlanarHull (hull_points); prism.setHeightLimits (z_min, z_max); prism.segment (cloud_indices); pcl::copyPointCloud(*scene_, cloud_indices, *above_plane_cloud); pcl::visualization::PCLVisualizer::Ptr vis; int vp1, vp2; if(!vis) { vis.reset (new pcl::visualization::PCLVisualizer("plane22 visualization")); vis->createViewPort(0,0,0.5,1,vp1); vis->createViewPort(0.5,0,1,1,vp2); } vis->removeAllPointClouds(); vis->removeAllShapes(); vis->addPointCloud(scene_, "cloud", vp1); vis->addPointCloud(above_plane_cloud, "convex_hull", vp2); vis->spin(); all_planes_[i]->visualize(); } }
void process () { std::cout << "threshold: " << threshold_ << std::endl; std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n"); 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::IntegralImageNormalEstimation<PointT, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.02f); ne.setNormalSmoothingSize (20.0f); typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ()); refinement_compare->setDistanceThreshold (threshold_, depth_dependent_); pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (5000); mps.setAngularThreshold (0.017453 * 3.0); //3 degrees mps.setDistanceThreshold (0.03); //2cm mps.setRefinementComparator (refinement_compare); std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; typename pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>); typename pcl::PointCloud<PointT>::Ptr approx_contour (new pcl::PointCloud<PointT>); char name[1024]; typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); double normal_start = pcl::getTime (); ne.setInputCloud (cloud); ne.compute (*normal_cloud); double normal_end = pcl::getTime (); std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl; double plane_extract_start = pcl::getTime (); mps.setInputNormals (normal_cloud); mps.setInputCloud (cloud); if (refine_) mps.segmentAndRefine (regions); else mps.segment (regions); double plane_extract_end = pcl::getTime (); std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << " with planar regions found: " << regions.size () << std::endl; std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl; typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>); viewer.removeAllPointClouds (0); viewer.removeAllShapes (0); pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0); viewer.addPointCloud<PointT> (cloud, single_color, "cloud"); pcl::PlanarPolygon<PointT> approx_polygon; //Draw Visualization for (size_t i = 0; i < regions.size (); i++) { Eigen::Vector3f centroid = regions[i].getCentroid (); Eigen::Vector4f model = regions[i].getCoefficients (); pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]); pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]), centroid[1] + (0.5f * model[1]), centroid[2] + (0.5f * model[2])); sprintf (name, "normal_%d", unsigned (i)); viewer.addArrow (pt2, pt1, 1.0, 0, 0, std::string (name)); contour->points = regions[i].getContour (); sprintf (name, "plane_%02d", int (i)); pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]); viewer.addPointCloud (contour, color, name); pcl::approximatePolygon (regions[i], approx_polygon, threshold_, polygon_refinement_); approx_contour->points = approx_polygon.getContour (); std::cout << "polygon: " << contour->size () << " -> " << approx_contour->size () << std::endl; typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour; // sprintf (name, "approx_plane_%02d", int (i)); // viewer.addPolygon<PointT> (approx_contour_const, 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name); for (unsigned idx = 0; idx < approx_contour->points.size (); ++idx) { sprintf (name, "approx_plane_%02d_%03d", int (i), int(idx)); viewer.addLine (approx_contour->points [idx], approx_contour->points [(idx+1)%approx_contour->points.size ()], 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name); } } }
void v4r::MultiPlaneSegmentation<PointT>::segment(bool force_unorganized) { models_.clear(); if(input_->isOrganized() && !force_unorganized) { pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); if(!normals_set_) { pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.02f); ne.setNormalSmoothingSize (20.0f); ne.setBorderPolicy (pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE); ne.setInputCloud (input_); ne.compute (*normal_cloud); } else { normal_cloud = normal_cloud_; } pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (min_plane_inliers_); mps.setAngularThreshold (0.017453 * 2.f); // 2 degrees mps.setDistanceThreshold (0.01); // 1cm mps.setMaximumCurvature(0.002); mps.setInputNormals (normal_cloud); mps.setInputCloud (input_); 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; typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp ( new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ()); ref_comp->setDistanceThreshold (0.01f, false); ref_comp->setAngularThreshold (0.017453 * 2.f); mps.setRefinementComparator (ref_comp); mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices); //mps.segment (model_coefficients, inlier_indices); //std::cout << model_coefficients.size() << std::endl; if(merge_planes_) { //sort planes by size //check if the first plane can be merged against the others, if yes, define a new plane combining both and add it to the cue typedef boost::adjacency_matrix<boost::undirectedS, int> GraphPlane; GraphPlane mergeable_planes (model_coefficients.size ()); for(size_t i=0; i < model_coefficients.size(); i++) { Eigen::Vector3f plane_i = Eigen::Vector3f (model_coefficients[i].values[0], model_coefficients[i].values[1], model_coefficients[i].values[2]); plane_i.normalize(); for(size_t j=(i+1); j < model_coefficients.size(); j++) { Eigen::Vector3f plane_j = Eigen::Vector3f (model_coefficients[j].values[0], model_coefficients[j].values[1], model_coefficients[j].values[2]); plane_j.normalize(); //std::cout << "dot product:" << plane_i.dot(plane_j) << " diff:" << std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) << std::endl; if(plane_i.dot(plane_j) > 0.95) { if(std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) < 0.015) { boost::add_edge (static_cast<int>(i), static_cast<int>(j), mergeable_planes); } } } } boost::vector_property_map<int> components (boost::num_vertices (mergeable_planes)); int n_cc = static_cast<int> (boost::connected_components (mergeable_planes, &components[0])); std::vector<int> cc_sizes; std::vector< std::vector<int> > cc_to_model_coeff; cc_sizes.resize (n_cc, 0); cc_to_model_coeff.resize(n_cc); for (size_t i = 0; i < model_coefficients.size (); i++) { cc_sizes[components[i]]++; cc_to_model_coeff[components[i]].push_back(i); } std::vector<pcl::ModelCoefficients> new_model_coefficients; std::vector<pcl::PointIndices> new_inlier_indices; for(size_t i=0; i < cc_sizes.size(); i++) { if(cc_sizes[i] < 2) { new_model_coefficients.push_back(model_coefficients[cc_to_model_coeff[i][0]]); new_inlier_indices.push_back(inlier_indices[cc_to_model_coeff[i][0]]); continue; } //std::cout << "going to merge CC:" << cc_sizes[i] << std::endl; pcl::ModelCoefficients model_coeff; model_coeff.values.resize(4); for(size_t k=0; k < 4; k++) model_coeff.values[k] = 0.f; pcl::PointIndices merged_indices; for(size_t j=0; j < cc_to_model_coeff[i].size(); j++) { for(size_t k=0; k < 4; k++) model_coeff.values[k] += model_coefficients[cc_to_model_coeff[i][j]].values[k]; merged_indices.indices.insert(merged_indices.indices.end(), inlier_indices[cc_to_model_coeff[i][j]].indices.begin(), inlier_indices[cc_to_model_coeff[i][j]].indices.end()); } for(size_t k=0; k < 4; k++) model_coeff.values[k] /= static_cast<float>(cc_to_model_coeff[i].size()); new_model_coefficients.push_back(model_coeff); new_inlier_indices.push_back(merged_indices); } model_coefficients = new_model_coefficients; inlier_indices = new_inlier_indices; } for(size_t i=0; i < model_coefficients.size(); i++) { PlaneModel<PointT> pm; pm.coefficients_ = model_coefficients[i]; pm.cloud_ = input_; pm.inliers_ = inlier_indices[i]; //recompute coefficients based on distance to camera and normal? Eigen::Vector4f centroid; pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid); Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]); Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3); float sum_w = 0.f; for(size_t k=0; k < inlier_indices[i].indices.size(); k++) { const int &idx = inlier_indices[i].indices[k]; float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm(); float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f); //w_k = 1.f; M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c); sum_w += w_k; } Eigen::Matrix3f scatter; scatter.setZero (); scatter = M_w.transpose() * M_w; Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV); //std::cout << svd.matrixV() << std::endl; Eigen::Vector3f n = svd.matrixV().col(2); //flip normal if required if(n.dot(c*-1) < 0) n = n * -1.f; float d = n.dot(c) * -1.f; //std::cout << "normal:" << n << std::endl; //std::cout << "d:" << d << std::endl; pm.coefficients_.values[0] = n[0]; pm.coefficients_.values[1] = n[1]; pm.coefficients_.values[2] = n[2]; pm.coefficients_.values[3] = d; pcl::PointIndices clean_inlier_indices; float dist_threshold_ = 0.01f; for(size_t k=0; k < inlier_indices[i].indices.size(); k++) { const int &idx = inlier_indices[i].indices[k]; Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap(); float val = n.dot(p) + d; if(std::abs(val) <= dist_threshold_) clean_inlier_indices.indices.push_back( idx ); } pm.inliers_ = clean_inlier_indices; models_.push_back(pm); } } else { // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<PointT> vg; PointTCloudPtr cloud_filtered (new PointTCloud); vg.setInputCloud (input_); float leaf_size_ = 0.005f; float dist_threshold_ = 0.01f; vg.setLeafSize (leaf_size_, leaf_size_, leaf_size_); vg.filter (*cloud_filtered); std::vector<bool> pixel_has_not_been_labelled( cloud_filtered->points.size(), true ); // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<PointT> seg; pcl::ModelCoefficients coefficients; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (dist_threshold_); PointTCloudPtr cloud_filtered_leftover (new PointTCloud (*cloud_filtered)); while (1) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered_leftover); pcl::PointIndices inliers_in_leftover; seg.segment (inliers_in_leftover, coefficients); std::cout << "inliers in left over: " << inliers_in_leftover.indices.size() << " of " << cloud_filtered_leftover->points.size() << std::endl; if ( (int)inliers_in_leftover.indices.size () < min_plane_inliers_) // Could not estimate a(nother) planar model big enough for the given cloud. break; // make indices correspond to complete downsampled cloud pcl::PointIndices indices_in_original_cloud; size_t current_index_in_leftover = 0; size_t px=0; indices_in_original_cloud.indices.resize(inliers_in_leftover.indices.size()); for(size_t inl_id=0; inl_id < inliers_in_leftover.indices.size(); inl_id++) { // assumes indices are sorted in ascending order bool found = false; do { if( pixel_has_not_been_labelled[px] ) { if( current_index_in_leftover == inliers_in_leftover.indices [inl_id] ) { indices_in_original_cloud.indices[ inl_id ] = px; found = true; } current_index_in_leftover++; } px++; } while( !found ); } for(size_t i=0; i<indices_in_original_cloud.indices.size(); i++) pixel_has_not_been_labelled[ indices_in_original_cloud.indices[i] ] = false; //save coefficients PlaneModel<PointT> pm; pm.coefficients_ = coefficients; pm.cloud_ = cloud_filtered; pm.inliers_ = indices_in_original_cloud; models_.push_back(pm); pcl::copyPointCloud(*cloud_filtered, pixel_has_not_been_labelled, *cloud_filtered_leftover); } std::cout << "Number of planes found:" << models_.size() << "organized:" << static_cast<int>(input_->isOrganized() && !force_unorganized) << std::endl; } }
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]); } } }