void pcl::RegionGrowingHSV<PointT, NormalT>::applyRegionMergingAlgorithm() { int number_of_points = static_cast<int> (indices_->size()); // calculate color of each segment std::vector< std::vector<float> > segment_color; std::vector<float> color; color.resize(3, 0); segment_color.resize(number_of_segments_, color); for (int i_point = 0; i_point < number_of_points; i_point++) { int point_index = (*indices_)[i_point]; int segment_index = point_labels_[point_index]; segment_color[segment_index][0] += input_->points[point_index].h; segment_color[segment_index][1] += input_->points[point_index].s; segment_color[segment_index][2] += input_->points[point_index].v; } for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) { segment_color[i_seg][0] = static_cast<unsigned int> (static_cast<float> (segment_color[i_seg][0]) / static_cast<float> (num_pts_in_segment_[i_seg])); segment_color[i_seg][1] = static_cast<unsigned int> (static_cast<float> (segment_color[i_seg][1]) / static_cast<float> (num_pts_in_segment_[i_seg])); segment_color[i_seg][2] = static_cast<unsigned int> (static_cast<float> (segment_color[i_seg][2]) / static_cast<float> (num_pts_in_segment_[i_seg])); } // now it is time to find out if there are segments with a similar color // and merge them together std::vector<unsigned int> num_pts_in_homogeneous_region; std::vector<int> num_seg_in_homogeneous_region; segment_labels_.resize(number_of_segments_, -1); float dist_thresh = distance_threshold_; int homogeneous_region_number = 0; int curr_homogeneous_region = 0; for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) { curr_homogeneous_region = 0; if (segment_labels_[i_seg] == -1) { segment_labels_[i_seg] = homogeneous_region_number; curr_homogeneous_region = homogeneous_region_number; num_pts_in_homogeneous_region.push_back(num_pts_in_segment_[i_seg]); num_seg_in_homogeneous_region.push_back(1); homogeneous_region_number++; } else curr_homogeneous_region = segment_labels_[i_seg]; unsigned int i_nghbr = 0; while (i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size()) { int index = segment_neighbours_[i_seg][i_nghbr]; if (segment_distances_[i_seg][i_nghbr] > dist_thresh) { i_nghbr++; continue; } if (segment_labels_[index] == -1) { bool similar_enough = calculateColorimetricalDifference(segment_color[i_seg], segment_color[index], true); if (similar_enough) { segment_labels_[index] = curr_homogeneous_region; num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index]; num_seg_in_homogeneous_region[curr_homogeneous_region] += 1; } } i_nghbr++; }// next neighbour }// next segment segment_color.clear(); color.clear(); std::vector< std::vector<int> > final_segments; std::vector<int> region; final_segments.resize(homogeneous_region_number, region); for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++) { final_segments[i_reg].resize(num_seg_in_homogeneous_region[i_reg], 0); } std::vector<int> counter; counter.resize(homogeneous_region_number, 0); for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) { int index = segment_labels_[i_seg]; final_segments[index][counter[index]] = i_seg; counter[index] += 1; } std::vector< std::vector< std::pair<float, int> > > region_neighbours; findRegionNeighbours(region_neighbours, final_segments); int final_segment_number = homogeneous_region_number; for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++) { if (num_pts_in_homogeneous_region[i_reg] < min_pts_per_cluster_) { if (region_neighbours[i_reg].empty()) continue; int nearest_neighbour = region_neighbours[i_reg][0].second; if (region_neighbours[i_reg][0].first == std::numeric_limits<float>::max()) continue; int reg_index = segment_labels_[nearest_neighbour]; int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg]; for (int i_seg = 0; i_seg < num_seg_in_reg; i_seg++) { int segment_index = final_segments[i_reg][i_seg]; final_segments[reg_index].push_back(segment_index); segment_labels_[segment_index] = reg_index; } final_segments[i_reg].clear(); num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg]; num_pts_in_homogeneous_region[i_reg] = 0; num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg]; num_seg_in_homogeneous_region[i_reg] = 0; final_segment_number -= 1; int nghbr_number = static_cast<int> (region_neighbours[reg_index].size()); for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++) { if (segment_labels_[region_neighbours[reg_index][i_nghbr].second] == reg_index) { region_neighbours[reg_index][i_nghbr].first = std::numeric_limits<float>::max(); region_neighbours[reg_index][i_nghbr].second = 0; } } nghbr_number = static_cast<int> (region_neighbours[i_reg].size()); for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++) { if (segment_labels_[region_neighbours[i_reg][i_nghbr].second] != reg_index) { std::pair<float, int> pair; pair.first = region_neighbours[i_reg][i_nghbr].first; pair.second = region_neighbours[i_reg][i_nghbr].second; region_neighbours[reg_index].push_back(pair); } } region_neighbours[i_reg].clear(); std::sort(region_neighbours[reg_index].begin(), region_neighbours[reg_index].end(), comparePair); } } assembleRegions(num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size())); number_of_segments_ = final_segment_number; }
bool pcl::RegionGrowingHSV<PointT, NormalT>::validatePoint(int initial_seed, int point, int nghbr, bool& is_a_seed) const { is_a_seed = true; // check the color difference std::vector<float> point_color; point_color.resize(3, 0); std::vector<float> nghbr_color; nghbr_color.resize(3, 0); point_color[0] = input_->points[point].h; point_color[1] = input_->points[point].s; point_color[2] = input_->points[point].v; nghbr_color[0] = input_->points[nghbr].h; nghbr_color[1] = input_->points[nghbr].s; nghbr_color[2] = input_->points[nghbr].v; bool similar_enough = calculateColorimetricalDifference(point_color, nghbr_color, false); if (!similar_enough) return (false); float cosine_threshold = cosf(theta_threshold_); // check the angle between normals if needed if (normal_flag_) { float data[4]; data[0] = input_->points[point].data[0]; data[1] = input_->points[point].data[1]; data[2] = input_->points[point].data[2]; data[3] = input_->points[point].data[3]; Eigen::Map<Eigen::Vector3f> initial_point(static_cast<float*> (data)); Eigen::Map<Eigen::Vector3f> initial_normal(static_cast<float*> (normals_->points[point].normal)); if (smooth_mode_flag_ == true) { Eigen::Map<Eigen::Vector3f> nghbr_normal(static_cast<float*> (normals_->points[nghbr].normal)); float dot_product = fabsf(nghbr_normal.dot(initial_normal)); if (dot_product < cosine_threshold) return (false); } else { Eigen::Map<Eigen::Vector3f> nghbr_normal(static_cast<float*> (normals_->points[nghbr].normal)); Eigen::Map<Eigen::Vector3f> initial_seed_normal(static_cast<float*> (normals_->points[initial_seed].normal)); float dot_product = fabsf(nghbr_normal.dot(initial_seed_normal)); if (dot_product < cosine_threshold) return (false); } } // check the curvature if needed if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) is_a_seed = false; // check the residual if needed if (residual_flag_) { float data_p[4]; data_p[0] = input_->points[point].data[0]; data_p[1] = input_->points[point].data[1]; data_p[2] = input_->points[point].data[2]; data_p[3] = input_->points[point].data[3]; float data_n[4]; data_n[0] = input_->points[nghbr].data[0]; data_n[1] = input_->points[nghbr].data[1]; data_n[2] = input_->points[nghbr].data[2]; data_n[3] = input_->points[nghbr].data[3]; Eigen::Map<Eigen::Vector3f> nghbr_point(static_cast<float*> (data_n)); Eigen::Map<Eigen::Vector3f> initial_point(static_cast<float*> (data_p)); Eigen::Map<Eigen::Vector3f> initial_normal(static_cast<float*> (normals_->points[point].normal)); float residual = fabsf(initial_normal.dot(initial_point - nghbr_point)); if (residual > residual_threshold_) is_a_seed = false; } return (true); }
template <typename PointT, typename NormalT> bool pcl::RegionGrowingRGB2<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const { is_a_seed = true; // check the color difference std::vector<unsigned int> point_color; point_color.resize (3, 0); std::vector<unsigned int> nghbr_color; nghbr_color.resize (3, 0); point_color[0] = input_->points[point].r; point_color[1] = input_->points[point].g; point_color[2] = input_->points[point].b; nghbr_color[0] = input_->points[nghbr].r; nghbr_color[1] = input_->points[nghbr].g; nghbr_color[2] = input_->points[nghbr].b; float difference = calculateColorimetricalDifference (point_color, nghbr_color); if (difference > color_p2p_threshold_) return (false); float cosine_threshold = cosf (theta_threshold_); // check the angle between normals if needed if (normal_flag_) { // float data[4]; // data[0] = input_->points[point].data[0]; // data[1] = input_->points[point].data[1]; // data[2] = input_->points[point].data[2]; // data[3] = input_->points[point].data[3]; // // Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data)); // Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal)); // if (smooth_mode_flag_ == true) // { // Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); // float dot_product = fabsf (nghbr_normal.dot (initial_normal)); // if (dot_product < cosine_threshold) // return (false); // } // else // { // Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); // Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal)); // float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal)); // if (dot_product < cosine_threshold) // return (false); // } } // check the curvature if needed if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) is_a_seed = false; // check the residual if needed if (residual_flag_) { float data_p[4]; data_p[0] = input_->points[point].data[0]; data_p[1] = input_->points[point].data[1]; data_p[2] = input_->points[point].data[2]; data_p[3] = input_->points[point].data[3]; float data_n[4]; data_n[0] = input_->points[nghbr].data[0]; data_n[1] = input_->points[nghbr].data[1]; data_n[2] = input_->points[nghbr].data[2]; data_n[3] = input_->points[nghbr].data[3]; Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n)); Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p)); Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal)); float residual = 0;//fabsf (initial_normal.dot (initial_point - nghbr_point)); if (residual > residual_threshold_) is_a_seed = false; } return (true); }