40 #ifndef PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
41 #define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
43 #include <pcl/segmentation/region_growing_rgb.h>
44 #include <pcl/search/search.h>
45 #include <pcl/search/kdtree.h>
50 template <
typename Po
intT,
typename NormalT>
52 color_p2p_threshold_ (1225.0f),
53 color_r2r_threshold_ (10.0f),
54 distance_threshold_ (0.05f),
55 region_neighbour_number_ (100),
57 segment_neighbours_ (0),
58 segment_distances_ (0),
68 template <
typename Po
intT,
typename NormalT>
71 point_distances_.clear ();
72 segment_neighbours_.clear ();
73 segment_distances_.clear ();
74 segment_labels_.clear ();
78 template <
typename Po
intT,
typename NormalT>
float
81 return (powf (color_p2p_threshold_, 0.5f));
85 template <
typename Po
intT,
typename NormalT>
void
88 color_p2p_threshold_ = thresh * thresh;
92 template <
typename Po
intT,
typename NormalT>
float
95 return (powf (color_r2r_threshold_, 0.5f));
99 template <
typename Po
intT,
typename NormalT>
void
102 color_r2r_threshold_ = thresh * thresh;
106 template <
typename Po
intT,
typename NormalT>
float
109 return (powf (distance_threshold_, 0.5f));
113 template <
typename Po
intT,
typename NormalT>
void
116 distance_threshold_ = thresh * thresh;
120 template <
typename Po
intT,
typename NormalT>
unsigned int
123 return (region_neighbour_number_);
127 template <
typename Po
intT,
typename NormalT>
void
130 region_neighbour_number_ = nghbr_number;
134 template <
typename Po
intT,
typename NormalT>
bool
137 return (normal_flag_);
141 template <
typename Po
intT,
typename NormalT>
void
144 normal_flag_ = value;
148 template <
typename Po
intT,
typename NormalT>
void
151 curvature_flag_ = value;
155 template <
typename Po
intT,
typename NormalT>
void
158 residual_flag_ = value;
162 template <
typename Po
intT,
typename NormalT>
void
167 point_neighbours_.clear ();
168 point_labels_.clear ();
169 num_pts_in_segment_.clear ();
170 point_distances_.clear ();
171 segment_neighbours_.clear ();
172 segment_distances_.clear ();
173 segment_labels_.clear ();
174 number_of_segments_ = 0;
176 bool segmentation_is_possible = initCompute ();
177 if ( !segmentation_is_possible )
183 segmentation_is_possible = prepareForSegmentation ();
184 if ( !segmentation_is_possible )
190 findPointNeighbours ();
191 applySmoothRegionGrowingAlgorithm ();
194 findSegmentNeighbours ();
195 applyRegionMergingAlgorithm ();
197 std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin ();
198 while (cluster_iter != clusters_.end ())
200 if (static_cast<int> (cluster_iter->indices.size ()) < min_pts_per_cluster_ ||
201 static_cast<int> (cluster_iter->indices.size ()) > max_pts_per_cluster_)
203 cluster_iter = clusters_.erase (cluster_iter);
209 clusters.reserve (clusters_.size ());
210 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
216 template <
typename Po
intT,
typename NormalT>
bool
220 if ( input_->points.empty () )
228 if ( !normals_ || input_->points.size () != normals_->points.size () )
235 if (residual_threshold_ <= 0.0f)
247 if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f )
251 if (neighbour_number_ == 0)
260 if (indices_->empty ())
261 PCL_ERROR (
"[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
262 search_->setInputCloud (input_, indices_);
265 search_->setInputCloud (input_);
271 template <
typename Po
intT,
typename NormalT>
void
274 int point_number =
static_cast<int> (indices_->size ());
275 std::vector<int> neighbours;
276 std::vector<float> distances;
278 point_neighbours_.resize (input_->points.size (), neighbours);
279 point_distances_.resize (input_->points.size (), distances);
281 for (
int i_point = 0; i_point < point_number; i_point++)
283 int point_index = (*indices_)[i_point];
286 search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances);
287 point_neighbours_[point_index].swap (neighbours);
288 point_distances_[point_index].swap (distances);
293 template <
typename Po
intT,
typename NormalT>
void
296 std::vector<int> neighbours;
297 std::vector<float> distances;
298 segment_neighbours_.resize (number_of_segments_, neighbours);
299 segment_distances_.resize (number_of_segments_, distances);
301 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
303 std::vector<int> nghbrs;
304 std::vector<float> dist;
305 findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
306 segment_neighbours_[i_seg].swap (nghbrs);
307 segment_distances_[i_seg].swap (dist);
312 template <
typename Po
intT,
typename NormalT>
void
315 std::vector<float> distances;
316 float max_dist = std::numeric_limits<float>::max ();
317 distances.resize (clusters_.size (), max_dist);
319 int number_of_points = num_pts_in_segment_[index];
321 for (
int i_point = 0; i_point < number_of_points; i_point++)
323 int point_index = clusters_[index].indices[i_point];
324 int number_of_neighbours =
static_cast<int> (point_neighbours_[point_index].size ());
327 for (
int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
330 int segment_index = -1;
331 segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
333 if ( segment_index != index )
336 if (distances[segment_index] > point_distances_[point_index][i_nghbr])
337 distances[segment_index] = point_distances_[point_index][i_nghbr];
342 std::priority_queue<std::pair<float, int> > segment_neighbours;
343 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
345 if (distances[i_seg] < max_dist)
347 segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) );
348 if (
int (segment_neighbours.size ()) > nghbr_number)
349 segment_neighbours.pop ();
353 int size = std::min<int> (
static_cast<int> (segment_neighbours.size ()), nghbr_number);
354 nghbrs.resize (size, 0);
355 dist.resize (size, 0);
357 while ( !segment_neighbours.empty () && counter < nghbr_number )
359 dist[counter] = segment_neighbours.top ().first;
360 nghbrs[counter] = segment_neighbours.top ().second;
361 segment_neighbours.pop ();
367 template <
typename Po
intT,
typename NormalT>
void
370 int number_of_points =
static_cast<int> (indices_->size ());
373 std::vector< std::vector<unsigned int> > segment_color;
374 std::vector<unsigned int> color;
376 segment_color.resize (number_of_segments_, color);
378 for (
int i_point = 0; i_point < number_of_points; i_point++)
380 int point_index = (*indices_)[i_point];
381 int segment_index = point_labels_[point_index];
382 segment_color[segment_index][0] += input_->points[point_index].r;
383 segment_color[segment_index][1] += input_->points[point_index].g;
384 segment_color[segment_index][2] += input_->points[point_index].b;
386 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
388 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]));
389 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]));
390 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]));
395 std::vector<unsigned int> num_pts_in_homogeneous_region;
396 std::vector<int> num_seg_in_homogeneous_region;
398 segment_labels_.resize (number_of_segments_, -1);
400 float dist_thresh = distance_threshold_;
401 int homogeneous_region_number = 0;
402 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
404 int curr_homogeneous_region = 0;
405 if (segment_labels_[i_seg] == -1)
407 segment_labels_[i_seg] = homogeneous_region_number;
408 curr_homogeneous_region = homogeneous_region_number;
409 num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
410 num_seg_in_homogeneous_region.push_back (1);
411 homogeneous_region_number++;
414 curr_homogeneous_region = segment_labels_[i_seg];
416 unsigned int i_nghbr = 0;
417 while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
419 int index = segment_neighbours_[i_seg][i_nghbr];
420 if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
425 if ( segment_labels_[index] == -1 )
427 float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
428 if (difference < color_r2r_threshold_)
430 segment_labels_[index] = curr_homogeneous_region;
431 num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
432 num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
439 segment_color.clear ();
442 std::vector< std::vector<int> > final_segments;
443 std::vector<int> region;
444 final_segments.resize (homogeneous_region_number, region);
445 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
447 final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
450 std::vector<int> counter;
451 counter.resize (homogeneous_region_number, 0);
452 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
454 int index = segment_labels_[i_seg];
455 final_segments[ index ][ counter[index] ] = i_seg;
459 std::vector< std::vector< std::pair<float, int> > > region_neighbours;
460 findRegionNeighbours (region_neighbours, final_segments);
462 int final_segment_number = homogeneous_region_number;
463 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
465 if (static_cast<int> (num_pts_in_homogeneous_region[i_reg]) < min_pts_per_cluster_)
467 if ( region_neighbours[i_reg].empty () )
469 int nearest_neighbour = region_neighbours[i_reg][0].second;
470 if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
472 int reg_index = segment_labels_[nearest_neighbour];
473 int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
474 for (
int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
476 int segment_index = final_segments[i_reg][i_seg];
477 final_segments[reg_index].push_back (segment_index);
478 segment_labels_[segment_index] = reg_index;
480 final_segments[i_reg].clear ();
481 num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
482 num_pts_in_homogeneous_region[i_reg] = 0;
483 num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
484 num_seg_in_homogeneous_region[i_reg] = 0;
485 final_segment_number -= 1;
487 int nghbr_number =
static_cast<int> (region_neighbours[reg_index].size ());
488 for (
int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
490 if ( segment_labels_[ region_neighbours[reg_index][i_nghbr].second ] == reg_index )
492 region_neighbours[reg_index][i_nghbr].first = std::numeric_limits<float>::max ();
493 region_neighbours[reg_index][i_nghbr].second = 0;
496 nghbr_number =
static_cast<int> (region_neighbours[i_reg].size ());
497 for (
int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
499 if ( segment_labels_[ region_neighbours[i_reg][i_nghbr].second ] != reg_index )
501 std::pair<float, int> pair;
502 pair.first = region_neighbours[i_reg][i_nghbr].first;
503 pair.second = region_neighbours[i_reg][i_nghbr].second;
504 region_neighbours[reg_index].push_back (pair);
507 region_neighbours[i_reg].clear ();
508 std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (),
comparePair);
512 assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ()));
514 number_of_segments_ = final_segment_number;
518 template <
typename Po
intT,
typename NormalT>
float
521 float difference = 0.0f;
522 difference += float ((first_color[0] - second_color[0]) * (first_color[0] - second_color[0]));
523 difference += float ((first_color[1] - second_color[1]) * (first_color[1] - second_color[1]));
524 difference += float ((first_color[2] - second_color[2]) * (first_color[2] - second_color[2]));
529 template <
typename Po
intT,
typename NormalT>
void
532 int region_number =
static_cast<int> (regions_in.size ());
533 neighbours_out.clear ();
534 neighbours_out.resize (region_number);
536 for (
int i_reg = 0; i_reg < region_number; i_reg++)
538 int segment_num =
static_cast<int> (regions_in[i_reg].size ());
539 neighbours_out[i_reg].reserve (segment_num * region_neighbour_number_);
540 for (
int i_seg = 0; i_seg < segment_num; i_seg++)
542 int curr_segment = regions_in[i_reg][i_seg];
543 int nghbr_number =
static_cast<int> (segment_neighbours_[curr_segment].size ());
544 std::pair<float, int> pair;
545 for (
int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
547 int segment_index = segment_neighbours_[curr_segment][i_nghbr];
548 if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
550 if (segment_labels_[segment_index] != i_reg)
552 pair.first = segment_distances_[curr_segment][i_nghbr];
553 pair.second = segment_index;
554 neighbours_out[i_reg].push_back (pair);
558 std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (),
comparePair);
563 template <
typename Po
intT,
typename NormalT>
void
568 clusters_.resize (num_regions, segment);
569 for (
int i_seg = 0; i_seg < num_regions; i_seg++)
571 clusters_[i_seg].
indices.resize (num_pts_in_region[i_seg]);
574 std::vector<int> counter;
575 counter.resize (num_regions, 0);
576 int point_number =
static_cast<int> (indices_->size ());
577 for (
int i_point = 0; i_point < point_number; i_point++)
579 int point_index = (*indices_)[i_point];
580 int index = point_labels_[point_index];
581 index = segment_labels_[index];
582 clusters_[index].indices[ counter[index] ] = point_index;
587 if (clusters_.empty ())
590 std::vector<pcl::PointIndices>::iterator itr1, itr2;
591 itr1 = clusters_.begin ();
592 itr2 = clusters_.end () - 1;
596 while (!(itr1->indices.empty ()) && itr1 < itr2)
598 while ( itr2->indices.empty () && itr1 < itr2)
602 itr1->indices.swap (itr2->indices);
605 if (itr2->indices.empty ())
606 clusters_.erase (itr2, clusters_.end ());
610 template <
typename Po
intT,
typename NormalT>
bool
616 std::vector<unsigned int> point_color;
617 point_color.resize (3, 0);
618 std::vector<unsigned int> nghbr_color;
619 nghbr_color.resize (3, 0);
620 point_color[0] = input_->points[point].r;
621 point_color[1] = input_->points[point].g;
622 point_color[2] = input_->points[point].b;
623 nghbr_color[0] = input_->points[nghbr].r;
624 nghbr_color[1] = input_->points[nghbr].g;
625 nghbr_color[2] = input_->points[nghbr].b;
626 float difference = calculateColorimetricalDifference (point_color, nghbr_color);
627 if (difference > color_p2p_threshold_)
630 float cosine_threshold = std::cos (theta_threshold_);
636 data[0] = input_->points[point].data[0];
637 data[1] = input_->points[point].data[1];
638 data[2] = input_->points[point].data[2];
639 data[3] = input_->points[point].data[3];
641 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
642 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
643 if (smooth_mode_flag_ ==
true)
645 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
646 float dot_product = std::abs (nghbr_normal.dot (initial_normal));
647 if (dot_product < cosine_threshold)
652 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
653 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal));
654 float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
655 if (dot_product < cosine_threshold)
661 if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
668 data_p[0] = input_->points[point].data[0];
669 data_p[1] = input_->points[point].data[1];
670 data_p[2] = input_->points[point].data[2];
671 data_p[3] = input_->points[point].data[3];
673 data_n[0] = input_->points[nghbr].data[0];
674 data_n[1] = input_->points[nghbr].data[1];
675 data_n[2] = input_->points[nghbr].data[2];
676 data_n[3] = input_->points[nghbr].data[3];
677 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
678 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
679 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
680 float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
681 if (residual > residual_threshold_)
689 template <
typename Po
intT,
typename NormalT>
void
694 bool segmentation_is_possible = initCompute ();
695 if ( !segmentation_is_possible )
702 bool point_was_found =
false;
703 int number_of_points = static_cast <
int> (indices_->size ());
704 for (
int point = 0; point < number_of_points; point++)
705 if ( (*indices_)[point] == index)
707 point_was_found =
true;
713 if (clusters_.empty ())
716 point_neighbours_.clear ();
717 point_labels_.clear ();
718 num_pts_in_segment_.clear ();
719 point_distances_.clear ();
720 segment_neighbours_.clear ();
721 segment_distances_.clear ();
722 segment_labels_.clear ();
723 number_of_segments_ = 0;
725 segmentation_is_possible = prepareForSegmentation ();
726 if ( !segmentation_is_possible )
732 findPointNeighbours ();
733 applySmoothRegionGrowingAlgorithm ();
736 findSegmentNeighbours ();
737 applyRegionMergingAlgorithm ();
741 for (
auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
743 bool segment_was_found =
false;
744 for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
746 if (i_segment->indices[i_point] == index)
748 segment_was_found =
true;
750 cluster.
indices.reserve (i_segment->indices.size ());
751 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
755 if (segment_was_found)
765 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
float getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
RegionGrowingRGB()
Constructor that sets default values for member variables.
~RegionGrowingRGB()
Destructor that frees memory.
void getSegmentFromPoint(int index, pcl::PointIndices &cluster) override
For a given point this function builds a segment to which it belongs and returns this segment...
void setResidualTestFlag(bool value) override
Allows to turn on/off the residual test.
bool residual_flag_
If set to true then residual test will be done during segmentation.
void setNumberOfRegionNeighbours(unsigned int nghbr_number)
This method allows to set the number of neighbours that is used for finding neighbouring segments...
int min_pts_per_cluster_
Stores the minimum number of points that a cluster needs to contain in order to be considered valid...
bool validatePoint(int initial_seed, int point, int nghbr, bool &is_a_seed) const override
This function is checking if the point with index 'nghbr' belongs to the segment. ...
void findRegionsKNN(int index, int nghbr_number, std::vector< int > &nghbrs, std::vector< float > &dist)
This method finds K nearest neighbours of the given segment.
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use...
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
float calculateColorimetricalDifference(std::vector< unsigned int > &first_color, std::vector< unsigned int > &second_color) const
This method calculates the colorimetrical difference between two points.
float getDistanceThreshold() const
Returns the distance threshold.
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
bool prepareForSegmentation() override
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
void setDistanceThreshold(float thresh)
Allows to set distance threshold.
void findRegionNeighbours(std::vector< std::vector< std::pair< float, int > > > &neighbours_out, std::vector< std::vector< int > > ®ions_in)
This method assembles the array containing neighbours of each homogeneous region. ...
void assembleRegions()
This function simply assembles the regions from list of point labels.
void extract(std::vector< pcl::PointIndices > &clusters) override
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
bool normal_flag_
If set to true then normal/smoothness test will be done during segmentation.
void setNormalTestFlag(bool value)
Allows to turn on/off the smoothness test.
void setCurvatureTestFlag(bool value) override
Allows to turn on/off the curvature test.
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
void findPointNeighbours() override
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.