template <typename PointT, typename NormalT> void
pcl::RegionGrowing<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
{
clusters_.clear ();
clusters.clear ();
point_neighbours_.clear ();
point_labels_.clear ();
num_pts_in_segment_.clear ();
number_of_segments_ = 0;
bool segmentation_is_possible = initCompute ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
segmentation_is_possible = prepareForSegmentation ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
findPointNeighbours ();
applySmoothRegionGrowingAlgorithm ();
assembleRegions ();
clusters.resize (clusters_.size ());
std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin ();
for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); cluster_iter++)
{
if ((static_cast<int> (cluster_iter->indices.size ()) >= min_pts_per_cluster_) &&
(static_cast<int> (cluster_iter->indices.size ()) <= max_pts_per_cluster_))
{
*cluster_iter_input = *cluster_iter;
cluster_iter_input++;
}
}
clusters_ = std::vector<pcl::PointIndices> (clusters.begin (), cluster_iter_input);
clusters.resize(clusters_.size());
deinitCompute ();
}
template <typename PointT, typename NormalT> void
pcl::RegionGrowing<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
{
clusters_.clear ();
clusters.clear ();
point_neighbours_.clear ();
point_labels_.clear ();
num_pts_in_segment_.clear ();
number_of_segments_ = 0;
bool segmentation_is_possible = initCompute ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
segmentation_is_possible = prepareForSegmentation ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
findPointNeighbours ();
applySmoothRegionGrowingAlgorithm ();
assembleRegions ();
std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin ();
while (cluster_iter != clusters_.end ())
{
if (cluster_iter->indices.size () < min_pts_per_cluster_ || cluster_iter->indices.size () > max_pts_per_cluster_)
{
cluster_iter = clusters_.erase (cluster_iter);
}
else
cluster_iter++;
}
clusters.reserve (clusters_.size ());
std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
deinitCompute ();
}
template <typename PointT> unsigned int
pcl::RegionGrowing<PointT>::segmentPoints ()
{
number_of_segments_ = 0;
segments_.clear ();
point_labels_.clear ();
num_pts_in_segment_.clear ();
point_neighbours_.clear ();
bool segmentation_is_possible = prepareForSegmentation ();
if ( !segmentation_is_possible )
return (number_of_segments_);
findPointNeighbours ();
number_of_segments_ = applySmoothRegionGrowingAlgorithm ();
assembleRegions ();
return (number_of_segments_);
}
template <typename PointT, typename NormalT> void
pcl::RegionGrowing<PointT, NormalT>::getSegmentFromPoint (int index, pcl::PointIndices& cluster)
{
cluster.indices.clear ();
bool segmentation_is_possible = initCompute ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
// first of all we need to find out if this point belongs to cloud
bool point_was_found = false;
int number_of_points = static_cast <int> (indices_->size ());
for (size_t point = 0; point < number_of_points; point++)
if ( (*indices_)[point] == index)
{
point_was_found = true;
break;
}
if (point_was_found)
{
if (clusters_.empty ())
{
point_neighbours_.clear ();
point_labels_.clear ();
num_pts_in_segment_.clear ();
number_of_segments_ = 0;
segmentation_is_possible = prepareForSegmentation ();
if ( !segmentation_is_possible )
{
deinitCompute ();
return;
}
findPointNeighbours ();
applySmoothRegionGrowingAlgorithm ();
assembleRegions ();
}
// if we have already made the segmentation, then find the segment
// to which this point belongs
std::vector <pcl::PointIndices>::iterator i_segment;
for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
{
bool segment_was_found = false;
for (size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
{
if (i_segment->indices[i_point] == index)
{
segment_was_found = true;
cluster.indices.clear ();
cluster.indices.reserve (i_segment->indices.size ());
std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices));
break;
}
}
if (segment_was_found)
{
break;
}
}// next segment
}// end if point was found
deinitCompute ();
}
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;
}
