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 (cluster_iter->indices.size () < min_pts_per_cluster_ || cluster_iter->indices.size () > max_pts_per_cluster_)
202 cluster_iter = clusters_.erase (cluster_iter);
208 clusters.reserve (clusters_.size ());
209 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
215 template <
typename Po
intT,
typename NormalT>
bool
219 if ( input_->points.size () == 0 )
227 if ( normals_ == 0 || input_->points.size () != normals_->points.size () )
234 if (residual_threshold_ <= 0.0f)
246 if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f )
250 if (neighbour_number_ == 0)
259 if (indices_->empty ())
260 PCL_ERROR (
"[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
261 search_->setInputCloud (input_, indices_);
264 search_->setInputCloud (input_);
270 template <
typename Po
intT,
typename NormalT>
void
273 int point_number =
static_cast<int> (indices_->size ());
274 std::vector<int> neighbours;
275 std::vector<float> distances;
277 point_neighbours_.resize (input_->points.size (), neighbours);
278 point_distances_.resize (input_->points.size (), distances);
280 for (
int i_point = 0; i_point < point_number; i_point++)
282 int point_index = (*indices_)[i_point];
285 search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances);
286 point_neighbours_[point_index].swap (neighbours);
287 point_distances_[point_index].swap (distances);
292 template <
typename Po
intT,
typename NormalT>
void
295 std::vector<int> neighbours;
296 std::vector<float> distances;
297 segment_neighbours_.resize (number_of_segments_, neighbours);
298 segment_distances_.resize (number_of_segments_, distances);
300 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
302 std::vector<int> nghbrs;
303 std::vector<float> dist;
304 findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
305 segment_neighbours_[i_seg].swap (nghbrs);
306 segment_distances_[i_seg].swap (dist);
311 template <
typename Po
intT,
typename NormalT>
void
314 std::vector<float> distances;
315 float max_dist = std::numeric_limits<float>::max ();
316 distances.resize (clusters_.size (), max_dist);
318 int number_of_points = num_pts_in_segment_[index];
320 for (
int i_point = 0; i_point < number_of_points; i_point++)
322 int point_index = clusters_[index].indices[i_point];
323 int number_of_neighbours =
static_cast<int> (point_neighbours_[point_index].size ());
326 for (
int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
329 int segment_index = -1;
330 segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
332 if ( segment_index != index )
335 if (distances[segment_index] > point_distances_[point_index][i_nghbr])
336 distances[segment_index] = point_distances_[point_index][i_nghbr];
341 std::priority_queue<std::pair<float, int> > segment_neighbours;
342 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
344 if (distances[i_seg] < max_dist)
346 segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) );
347 if (
int (segment_neighbours.size ()) > nghbr_number)
348 segment_neighbours.pop ();
352 int size = std::min<int> (
static_cast<int> (segment_neighbours.size ()), nghbr_number);
353 nghbrs.resize (size, 0);
354 dist.resize (size, 0);
356 while ( !segment_neighbours.empty () && counter < nghbr_number )
358 dist[counter] = segment_neighbours.top ().first;
359 nghbrs[counter] = segment_neighbours.top ().second;
360 segment_neighbours.pop ();
366 template <
typename Po
intT,
typename NormalT>
void
369 int number_of_points =
static_cast<int> (indices_->size ());
372 std::vector< std::vector<unsigned int> > segment_color;
373 std::vector<unsigned int> color;
375 segment_color.resize (number_of_segments_, color);
377 for (
int i_point = 0; i_point < number_of_points; i_point++)
379 int point_index = (*indices_)[i_point];
380 int segment_index = point_labels_[point_index];
381 segment_color[segment_index][0] += input_->points[point_index].r;
382 segment_color[segment_index][1] += input_->points[point_index].g;
383 segment_color[segment_index][2] += input_->points[point_index].b;
385 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
387 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]));
388 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]));
389 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]));
394 std::vector<unsigned int> num_pts_in_homogeneous_region;
395 std::vector<int> num_seg_in_homogeneous_region;
397 segment_labels_.resize (number_of_segments_, -1);
399 float dist_thresh = distance_threshold_;
400 int homogeneous_region_number = 0;
401 int curr_homogeneous_region = 0;
402 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
404 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 (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 std::vector< pcl::PointIndices >::iterator i_region;
588 i_region = clusters_.begin ();
589 while(i_region != clusters_.end ())
591 if ( i_region->indices.empty () )
592 i_region = clusters_.erase (i_region);
599 template <
typename Po
intT,
typename NormalT>
bool
605 std::vector<unsigned int> point_color;
606 point_color.resize (3, 0);
607 std::vector<unsigned int> nghbr_color;
608 nghbr_color.resize (3, 0);
609 point_color[0] = input_->points[point].r;
610 point_color[1] = input_->points[point].g;
611 point_color[2] = input_->points[point].b;
612 nghbr_color[0] = input_->points[nghbr].r;
613 nghbr_color[1] = input_->points[nghbr].g;
614 nghbr_color[2] = input_->points[nghbr].b;
615 float difference = calculateColorimetricalDifference (point_color, nghbr_color);
616 if (difference > color_p2p_threshold_)
619 float cosine_threshold = cosf (theta_threshold_);
625 data[0] = input_->points[point].data[0];
626 data[1] = input_->points[point].data[1];
627 data[2] = input_->points[point].data[2];
628 data[3] = input_->points[point].data[3];
630 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
631 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
632 if (smooth_mode_flag_ ==
true)
634 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
635 float dot_product = fabsf (nghbr_normal.dot (initial_normal));
636 if (dot_product < cosine_threshold)
641 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
642 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal));
643 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
644 if (dot_product < cosine_threshold)
650 if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
657 data_p[0] = input_->points[point].data[0];
658 data_p[1] = input_->points[point].data[1];
659 data_p[2] = input_->points[point].data[2];
660 data_p[3] = input_->points[point].data[3];
662 data_n[0] = input_->points[nghbr].data[0];
663 data_n[1] = input_->points[nghbr].data[1];
664 data_n[2] = input_->points[nghbr].data[2];
665 data_n[3] = input_->points[nghbr].data[3];
666 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
667 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
668 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
669 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
670 if (residual > residual_threshold_)
678 template <
typename Po
intT,
typename NormalT>
void
683 bool segmentation_is_possible = initCompute ();
684 if ( !segmentation_is_possible )
691 bool point_was_found =
false;
692 int number_of_points = static_cast <
int> (indices_->size ());
693 for (
size_t point = 0; point < number_of_points; point++)
694 if ( (*indices_)[point] == index)
696 point_was_found =
true;
702 if (clusters_.empty ())
705 point_neighbours_.clear ();
706 point_labels_.clear ();
707 num_pts_in_segment_.clear ();
708 point_distances_.clear ();
709 segment_neighbours_.clear ();
710 segment_distances_.clear ();
711 segment_labels_.clear ();
712 number_of_segments_ = 0;
714 segmentation_is_possible = prepareForSegmentation ();
715 if ( !segmentation_is_possible )
721 findPointNeighbours ();
722 applySmoothRegionGrowingAlgorithm ();
725 findSegmentNeighbours ();
726 applyRegionMergingAlgorithm ();
730 std::vector <pcl::PointIndices>::iterator i_segment;
731 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
733 bool segment_was_found =
false;
734 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
736 if (i_segment->indices[i_point] == index)
738 segment_was_found =
true;
740 cluster.
indices.reserve (i_segment->indices.size ());
741 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
745 if (segment_was_found)
755 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
virtual void findPointNeighbours()
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
float getDistanceThreshold() const
Returns the distance threshold.
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
virtual void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
RegionGrowingRGB()
Constructor that sets default values for member variables.
virtual bool validatePoint(int initial_seed, int point, int nghbr, bool &is_a_seed) const
This function is checking if the point with index 'nghbr' belongs to the segment. ...
virtual void setResidualTestFlag(bool value)
Allows to turn on/off the residual test.
void setDistanceThreshold(float thresh)
Allows to set distance threshold.
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
int min_pts_per_cluster_
Stores the minimum number of points that a cluster needs to contain in order to be considered valid...
bool normal_flag_
If set to true then normal/smoothness test will be done during segmentation.
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.
virtual void setCurvatureTestFlag(bool value)
Allows to turn on/off the curvature test.
bool residual_flag_
If set to true then residual test will be done during segmentation.
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
std::vector< int > indices
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.
float getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
virtual bool prepareForSegmentation()
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
virtual ~RegionGrowingRGB()
Destructor that frees memory.
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 setNormalTestFlag(bool value)
Allows to turn on/off the smoothness test.
float calculateColorimetricalDifference(std::vector< unsigned int > &first_color, std::vector< unsigned int > &second_color) const
This method calculates the colorimetrical difference between two points.
void assembleRegions()
This function simply assembles the regions from list of point labels.
void setNumberOfRegionNeighbours(unsigned int nghbr_number)
This method allows to set the number of neighbours that is used for finding neighbouring segments...
virtual void getSegmentFromPoint(int index, pcl::PointIndices &cluster)
For a given point this function builds a segment to which it belongs and returns this segment...
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use...
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...