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.size () == 0 )
228 if ( normals_ == 0 || 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 int curr_homogeneous_region = 0;
403 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
405 curr_homogeneous_region = 0;
406 if (segment_labels_[i_seg] == -1)
408 segment_labels_[i_seg] = homogeneous_region_number;
409 curr_homogeneous_region = homogeneous_region_number;
410 num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
411 num_seg_in_homogeneous_region.push_back (1);
412 homogeneous_region_number++;
415 curr_homogeneous_region = segment_labels_[i_seg];
417 unsigned int i_nghbr = 0;
418 while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
420 int index = segment_neighbours_[i_seg][i_nghbr];
421 if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
426 if ( segment_labels_[index] == -1 )
428 float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
429 if (difference < color_r2r_threshold_)
431 segment_labels_[index] = curr_homogeneous_region;
432 num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
433 num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
440 segment_color.clear ();
443 std::vector< std::vector<int> > final_segments;
444 std::vector<int> region;
445 final_segments.resize (homogeneous_region_number, region);
446 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
448 final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
451 std::vector<int> counter;
452 counter.resize (homogeneous_region_number, 0);
453 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
455 int index = segment_labels_[i_seg];
456 final_segments[ index ][ counter[index] ] = i_seg;
460 std::vector< std::vector< std::pair<float, int> > > region_neighbours;
461 findRegionNeighbours (region_neighbours, final_segments);
463 int final_segment_number = homogeneous_region_number;
464 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
466 if (static_cast<int> (num_pts_in_homogeneous_region[i_reg]) < min_pts_per_cluster_)
468 if ( region_neighbours[i_reg].empty () )
470 int nearest_neighbour = region_neighbours[i_reg][0].second;
471 if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
473 int reg_index = segment_labels_[nearest_neighbour];
474 int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
475 for (
int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
477 int segment_index = final_segments[i_reg][i_seg];
478 final_segments[reg_index].push_back (segment_index);
479 segment_labels_[segment_index] = reg_index;
481 final_segments[i_reg].clear ();
482 num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
483 num_pts_in_homogeneous_region[i_reg] = 0;
484 num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
485 num_seg_in_homogeneous_region[i_reg] = 0;
486 final_segment_number -= 1;
488 int nghbr_number =
static_cast<int> (region_neighbours[reg_index].size ());
489 for (
int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
491 if ( segment_labels_[ region_neighbours[reg_index][i_nghbr].second ] == reg_index )
493 region_neighbours[reg_index][i_nghbr].first = std::numeric_limits<float>::max ();
494 region_neighbours[reg_index][i_nghbr].second = 0;
497 nghbr_number =
static_cast<int> (region_neighbours[i_reg].size ());
498 for (
int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
500 if ( segment_labels_[ region_neighbours[i_reg][i_nghbr].second ] != reg_index )
502 std::pair<float, int> pair;
503 pair.first = region_neighbours[i_reg][i_nghbr].first;
504 pair.second = region_neighbours[i_reg][i_nghbr].second;
505 region_neighbours[reg_index].push_back (pair);
508 region_neighbours[i_reg].clear ();
509 std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (),
comparePair);
513 assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ()));
515 number_of_segments_ = final_segment_number;
519 template <
typename Po
intT,
typename NormalT>
float 522 float difference = 0.0f;
523 difference += float ((first_color[0] - second_color[0]) * (first_color[0] - second_color[0]));
524 difference += float ((first_color[1] - second_color[1]) * (first_color[1] - second_color[1]));
525 difference += float ((first_color[2] - second_color[2]) * (first_color[2] - second_color[2]));
530 template <
typename Po
intT,
typename NormalT>
void 533 int region_number =
static_cast<int> (regions_in.size ());
534 neighbours_out.clear ();
535 neighbours_out.resize (region_number);
537 for (
int i_reg = 0; i_reg < region_number; i_reg++)
539 int segment_num =
static_cast<int> (regions_in[i_reg].size ());
540 neighbours_out[i_reg].reserve (segment_num * region_neighbour_number_);
541 for (
int i_seg = 0; i_seg < segment_num; i_seg++)
543 int curr_segment = regions_in[i_reg][i_seg];
544 int nghbr_number =
static_cast<int> (segment_neighbours_[curr_segment].size ());
545 std::pair<float, int> pair;
546 for (
int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
548 int segment_index = segment_neighbours_[curr_segment][i_nghbr];
549 if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
551 if (segment_labels_[segment_index] != i_reg)
553 pair.first = segment_distances_[curr_segment][i_nghbr];
554 pair.second = segment_index;
555 neighbours_out[i_reg].push_back (pair);
559 std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (),
comparePair);
564 template <
typename Po
intT,
typename NormalT>
void 569 clusters_.resize (num_regions, segment);
570 for (
int i_seg = 0; i_seg < num_regions; i_seg++)
572 clusters_[i_seg].
indices.resize (num_pts_in_region[i_seg]);
575 std::vector<int> counter;
576 counter.resize (num_regions, 0);
577 int point_number =
static_cast<int> (indices_->size ());
578 for (
int i_point = 0; i_point < point_number; i_point++)
580 int point_index = (*indices_)[i_point];
581 int index = point_labels_[point_index];
582 index = segment_labels_[index];
583 clusters_[index].indices[ counter[index] ] = point_index;
588 if (clusters_.empty ())
591 std::vector<pcl::PointIndices>::iterator itr1, itr2;
592 itr1 = clusters_.begin ();
593 itr2 = clusters_.end () - 1;
597 while (!(itr1->indices.empty ()) && itr1 < itr2)
599 while ( itr2->indices.empty () && itr1 < itr2)
603 itr1->indices.swap (itr2->indices);
606 if (itr2->indices.empty ())
607 clusters_.erase (itr2, clusters_.end ());
611 template <
typename Po
intT,
typename NormalT>
bool 617 std::vector<unsigned int> point_color;
618 point_color.resize (3, 0);
619 std::vector<unsigned int> nghbr_color;
620 nghbr_color.resize (3, 0);
621 point_color[0] = input_->points[point].r;
622 point_color[1] = input_->points[point].g;
623 point_color[2] = input_->points[point].b;
624 nghbr_color[0] = input_->points[nghbr].r;
625 nghbr_color[1] = input_->points[nghbr].g;
626 nghbr_color[2] = input_->points[nghbr].b;
627 float difference = calculateColorimetricalDifference (point_color, nghbr_color);
628 if (difference > color_p2p_threshold_)
631 float cosine_threshold = cosf (theta_threshold_);
637 data[0] = input_->points[point].data[0];
638 data[1] = input_->points[point].data[1];
639 data[2] = input_->points[point].data[2];
640 data[3] = input_->points[point].data[3];
642 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
643 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
644 if (smooth_mode_flag_ ==
true)
646 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
647 float dot_product = fabsf (nghbr_normal.dot (initial_normal));
648 if (dot_product < cosine_threshold)
653 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
654 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal));
655 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
656 if (dot_product < cosine_threshold)
662 if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
669 data_p[0] = input_->points[point].data[0];
670 data_p[1] = input_->points[point].data[1];
671 data_p[2] = input_->points[point].data[2];
672 data_p[3] = input_->points[point].data[3];
674 data_n[0] = input_->points[nghbr].data[0];
675 data_n[1] = input_->points[nghbr].data[1];
676 data_n[2] = input_->points[nghbr].data[2];
677 data_n[3] = input_->points[nghbr].data[3];
678 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
679 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
680 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
681 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
682 if (residual > residual_threshold_)
690 template <
typename Po
intT,
typename NormalT>
void 695 bool segmentation_is_possible = initCompute ();
696 if ( !segmentation_is_possible )
703 bool point_was_found =
false;
704 int number_of_points = static_cast <
int> (indices_->size ());
705 for (
int point = 0; point < number_of_points; point++)
706 if ( (*indices_)[point] == index)
708 point_was_found =
true;
714 if (clusters_.empty ())
717 point_neighbours_.clear ();
718 point_labels_.clear ();
719 num_pts_in_segment_.clear ();
720 point_distances_.clear ();
721 segment_neighbours_.clear ();
722 segment_distances_.clear ();
723 segment_labels_.clear ();
724 number_of_segments_ = 0;
726 segmentation_is_possible = prepareForSegmentation ();
727 if ( !segmentation_is_possible )
733 findPointNeighbours ();
734 applySmoothRegionGrowingAlgorithm ();
737 findSegmentNeighbours ();
738 applyRegionMergingAlgorithm ();
742 std::vector <pcl::PointIndices>::iterator i_segment;
743 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
745 bool segment_was_found =
false;
746 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
748 if (i_segment->indices[i_point] == index)
750 segment_was_found =
true;
752 cluster.
indices.reserve (i_segment->indices.size ());
753 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
757 if (segment_was_found)
767 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_ void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
RegionGrowingRGB()
Constructor that sets default values for member variables.
virtual ~RegionGrowingRGB()
Destructor that frees memory.
bool residual_flag_
If set to true then residual test will be done during segmentation.
std::vector< int > indices
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...
virtual void setResidualTestFlag(bool value)
Allows to turn on/off the residual test.
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.
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use...
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. ...
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 getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
float getDistanceThreshold() const
Returns the distance threshold.
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
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. ...
virtual void setCurvatureTestFlag(bool value)
Allows to turn on/off the curvature test.
void assembleRegions()
This function simply assembles the regions from list of point labels.
virtual void findPointNeighbours()
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
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.
virtual bool prepareForSegmentation()
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
virtual void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
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...
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.