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);
}
