40 #ifndef PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
41 #define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
43 #include <pcl/console/print.h>
44 #include <pcl/segmentation/region_growing_rgb.h>
45 #include <pcl/search/search.h>
46 #include <pcl/search/kdtree.h>
51 template <
typename Po
intT,
typename NormalT>
53 color_p2p_threshold_ (1225.0f),
54 color_r2r_threshold_ (10.0f),
55 distance_threshold_ (0.05f),
56 region_neighbour_number_ (100),
58 segment_neighbours_ (0),
59 segment_distances_ (0),
69 template <
typename Po
intT,
typename NormalT>
72 point_distances_.clear ();
73 segment_neighbours_.clear ();
74 segment_distances_.clear ();
75 segment_labels_.clear ();
79 template <
typename Po
intT,
typename NormalT>
float
82 return (powf (color_p2p_threshold_, 0.5f));
86 template <
typename Po
intT,
typename NormalT>
void
89 color_p2p_threshold_ = thresh * thresh;
93 template <
typename Po
intT,
typename NormalT>
float
96 return (powf (color_r2r_threshold_, 0.5f));
100 template <
typename Po
intT,
typename NormalT>
void
103 color_r2r_threshold_ = thresh * thresh;
107 template <
typename Po
intT,
typename NormalT>
float
110 return (powf (distance_threshold_, 0.5f));
114 template <
typename Po
intT,
typename NormalT>
void
117 distance_threshold_ = thresh * thresh;
121 template <
typename Po
intT,
typename NormalT>
unsigned int
124 return (region_neighbour_number_);
128 template <
typename Po
intT,
typename NormalT>
void
131 region_neighbour_number_ = nghbr_number;
135 template <
typename Po
intT,
typename NormalT>
bool
138 return (normal_flag_);
142 template <
typename Po
intT,
typename NormalT>
void
145 normal_flag_ = value;
149 template <
typename Po
intT,
typename NormalT>
void
152 curvature_flag_ = value;
156 template <
typename Po
intT,
typename NormalT>
void
159 residual_flag_ = value;
163 template <
typename Po
intT,
typename NormalT>
void
168 point_neighbours_.clear ();
169 point_labels_.clear ();
170 num_pts_in_segment_.clear ();
171 point_distances_.clear ();
172 segment_neighbours_.clear ();
173 segment_distances_.clear ();
174 segment_labels_.clear ();
175 number_of_segments_ = 0;
177 bool segmentation_is_possible = initCompute ();
178 if ( !segmentation_is_possible )
184 segmentation_is_possible = prepareForSegmentation ();
185 if ( !segmentation_is_possible )
191 findPointNeighbours ();
192 applySmoothRegionGrowingAlgorithm ();
195 findSegmentNeighbours ();
196 applyRegionMergingAlgorithm ();
198 auto cluster_iter = clusters_.begin ();
199 while (cluster_iter != clusters_.end ())
201 if (cluster_iter->indices.size () < min_pts_per_cluster_ ||
202 cluster_iter->indices.size () > max_pts_per_cluster_)
204 cluster_iter = clusters_.erase (cluster_iter);
210 clusters.reserve (clusters_.size ());
211 std::copy (clusters_.cbegin (), clusters_.cend (), std::back_inserter (clusters));
217 template <
typename Po
intT,
typename NormalT>
bool
221 if ( input_->points.empty () )
229 if ( !normals_ || input_->size () != normals_->size () )
236 if (residual_threshold_ <= 0.0f)
248 if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f )
252 if (neighbour_number_ == 0)
261 if (indices_->empty ())
262 PCL_ERROR (
"[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
263 search_->setInputCloud (input_, indices_);
266 search_->setInputCloud (input_);
272 template <
typename Po
intT,
typename NormalT>
void
275 int point_number =
static_cast<int> (indices_->size ());
277 std::vector<float> distances;
279 point_neighbours_.resize (input_->size (), neighbours);
280 point_distances_.resize (input_->size (), distances);
282 for (
int i_point = 0; i_point < point_number; i_point++)
284 int point_index = (*indices_)[i_point];
287 search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances);
288 point_neighbours_[point_index].swap (neighbours);
289 point_distances_[point_index].swap (distances);
294 template <
typename Po
intT,
typename NormalT>
void
298 std::vector<float> distances;
299 segment_neighbours_.resize (number_of_segments_, neighbours);
300 segment_distances_.resize (number_of_segments_, distances);
302 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
305 std::vector<float> dist;
306 findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
307 segment_neighbours_[i_seg].swap (nghbrs);
308 segment_distances_[i_seg].swap (dist);
313 template <
typename Po
intT,
typename NormalT>
void
316 std::vector<float> distances;
317 float max_dist = std::numeric_limits<float>::max ();
318 distances.resize (clusters_.size (), max_dist);
320 const auto number_of_points = num_pts_in_segment_[index];
322 for (
pcl::uindex_t i_point = 0; i_point < number_of_points; i_point++)
324 const auto point_index = clusters_[index].indices[i_point];
325 const auto number_of_neighbours = point_neighbours_[point_index].size ();
328 for (std::size_t i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
331 const pcl::index_t 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 (segment_neighbours.size () > nghbr_number)
349 segment_neighbours.pop ();
353 const std::size_t size = std::min<std::size_t> (segment_neighbours.size (),
static_cast<std::size_t
>(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
371 std::vector< std::vector<unsigned int> > segment_color;
372 std::vector<unsigned int> color;
374 segment_color.resize (number_of_segments_, color);
376 for (
const auto& point_index : (*indices_))
378 int segment_index = point_labels_[point_index];
379 segment_color[segment_index][0] += (*input_)[point_index].r;
380 segment_color[segment_index][1] += (*input_)[point_index].g;
381 segment_color[segment_index][2] += (*input_)[point_index].b;
383 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
385 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]));
386 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]));
387 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]));
392 std::vector<unsigned int> num_pts_in_homogeneous_region;
393 std::vector<int> num_seg_in_homogeneous_region;
395 segment_labels_.resize (number_of_segments_, -1);
397 float dist_thresh = distance_threshold_;
398 int homogeneous_region_number = 0;
399 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
401 int curr_homogeneous_region = 0;
402 if (segment_labels_[i_seg] == -1)
404 segment_labels_[i_seg] = homogeneous_region_number;
405 curr_homogeneous_region = homogeneous_region_number;
406 num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
407 num_seg_in_homogeneous_region.push_back (1);
408 homogeneous_region_number++;
411 curr_homogeneous_region = segment_labels_[i_seg];
413 unsigned int i_nghbr = 0;
414 while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
416 int index = segment_neighbours_[i_seg][i_nghbr];
417 if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
422 if ( segment_labels_[index] == -1 )
424 float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
425 if (difference < color_r2r_threshold_)
427 segment_labels_[index] = curr_homogeneous_region;
428 num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
429 num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
436 segment_color.clear ();
439 std::vector< std::vector<int> > final_segments;
440 std::vector<int> region;
441 final_segments.resize (homogeneous_region_number, region);
442 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
444 final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
447 std::vector<int> counter;
448 counter.resize (homogeneous_region_number, 0);
449 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
451 int index = segment_labels_[i_seg];
452 final_segments[ index ][ counter[index] ] = i_seg;
456 std::vector< std::vector< std::pair<float, pcl::index_t> > > region_neighbours;
457 findRegionNeighbours (region_neighbours, final_segments);
459 int final_segment_number = homogeneous_region_number;
460 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
462 if (num_pts_in_homogeneous_region[i_reg] < min_pts_per_cluster_)
464 if ( region_neighbours[i_reg].empty () )
466 int nearest_neighbour = region_neighbours[i_reg][0].second;
467 if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
469 int reg_index = segment_labels_[nearest_neighbour];
470 int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
471 for (
int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
473 int segment_index = final_segments[i_reg][i_seg];
474 final_segments[reg_index].push_back (segment_index);
475 segment_labels_[segment_index] = reg_index;
477 final_segments[i_reg].clear ();
478 num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
479 num_pts_in_homogeneous_region[i_reg] = 0;
480 num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
481 num_seg_in_homogeneous_region[i_reg] = 0;
482 final_segment_number -= 1;
484 for (
auto& nghbr : region_neighbours[reg_index])
486 if ( segment_labels_[ nghbr.second ] == reg_index )
488 nghbr.first = std::numeric_limits<float>::max ();
492 for (
const auto& nghbr : region_neighbours[i_reg])
494 if ( segment_labels_[ nghbr.second ] != reg_index )
496 region_neighbours[reg_index].push_back (nghbr);
499 region_neighbours[i_reg].clear ();
500 std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (),
comparePair);
504 assembleRegions (num_pts_in_homogeneous_region,
static_cast<int> (num_pts_in_homogeneous_region.size ()));
506 number_of_segments_ = final_segment_number;
510 template <
typename Po
intT,
typename NormalT>
float
513 float difference = 0.0f;
514 difference +=
static_cast<float>((first_color[0] - second_color[0]) * (first_color[0] - second_color[0]));
515 difference +=
static_cast<float>((first_color[1] - second_color[1]) * (first_color[1] - second_color[1]));
516 difference +=
static_cast<float>((first_color[2] - second_color[2]) * (first_color[2] - second_color[2]));
521 template <
typename Po
intT,
typename NormalT>
void
524 int region_number =
static_cast<int> (regions_in.size ());
525 neighbours_out.clear ();
526 neighbours_out.resize (region_number);
528 for (
int i_reg = 0; i_reg < region_number; i_reg++)
530 neighbours_out[i_reg].reserve (regions_in[i_reg].size () * region_neighbour_number_);
531 for (
const auto& curr_segment : regions_in[i_reg])
533 const std::size_t nghbr_number = segment_neighbours_[curr_segment].size ();
534 std::pair<float, pcl::index_t> pair;
535 for (std::size_t i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
537 const auto segment_index = segment_neighbours_[curr_segment][i_nghbr];
538 if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
540 if (segment_labels_[segment_index] != i_reg)
542 pair.first = segment_distances_[curr_segment][i_nghbr];
543 pair.second = segment_index;
544 neighbours_out[i_reg].push_back (pair);
548 std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (),
comparePair);
553 template <
typename Po
intT,
typename NormalT>
void
558 clusters_.resize (num_regions, segment);
559 for (
int i_seg = 0; i_seg < num_regions; i_seg++)
561 clusters_[i_seg].
indices.resize (num_pts_in_region[i_seg]);
564 std::vector<int> counter;
565 counter.resize (num_regions, 0);
566 for (
const auto& point_index : (*indices_))
568 int index = point_labels_[point_index];
569 index = segment_labels_[index];
570 clusters_[index].indices[ counter[index] ] = point_index;
575 if (clusters_.empty ())
578 std::vector<pcl::PointIndices>::iterator itr1, itr2;
579 itr1 = clusters_.begin ();
580 itr2 = clusters_.end () - 1;
584 while (!(itr1->indices.empty ()) && itr1 < itr2)
586 while ( itr2->indices.empty () && itr1 < itr2)
590 itr1->indices.swap (itr2->indices);
593 if (itr2->indices.empty ())
594 clusters_.erase (itr2, clusters_.end ());
598 template <
typename Po
intT,
typename NormalT>
bool
604 std::vector<unsigned int> point_color;
605 point_color.resize (3, 0);
606 std::vector<unsigned int> nghbr_color;
607 nghbr_color.resize (3, 0);
608 point_color[0] = (*input_)[point].r;
609 point_color[1] = (*input_)[point].g;
610 point_color[2] = (*input_)[point].b;
611 nghbr_color[0] = (*input_)[nghbr].r;
612 nghbr_color[1] = (*input_)[nghbr].g;
613 nghbr_color[2] = (*input_)[nghbr].b;
614 float difference = calculateColorimetricalDifference (point_color, nghbr_color);
615 if (difference > color_p2p_threshold_)
618 float cosine_threshold = std::cos (theta_threshold_);
624 data[0] = (*input_)[point].data[0];
625 data[1] = (*input_)[point].data[1];
626 data[2] = (*input_)[point].data[2];
627 data[3] = (*input_)[point].data[3];
629 Eigen::Map<Eigen::Vector3f> initial_point (
static_cast<float*
> (data));
630 Eigen::Map<Eigen::Vector3f> initial_normal (
static_cast<float*
> ((*normals_)[point].normal));
631 if (smooth_mode_flag_ ==
true)
633 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
634 float dot_product = std::abs (nghbr_normal.dot (initial_normal));
635 if (dot_product < cosine_threshold)
640 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
641 Eigen::Map<Eigen::Vector3f> initial_seed_normal (
static_cast<float*
> ((*normals_)[initial_seed].normal));
642 float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
643 if (dot_product < cosine_threshold)
649 if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
656 data_p[0] = (*input_)[point].data[0];
657 data_p[1] = (*input_)[point].data[1];
658 data_p[2] = (*input_)[point].data[2];
659 data_p[3] = (*input_)[point].data[3];
661 data_n[0] = (*input_)[nghbr].data[0];
662 data_n[1] = (*input_)[nghbr].data[1];
663 data_n[2] = (*input_)[nghbr].data[2];
664 data_n[3] = (*input_)[nghbr].data[3];
665 Eigen::Map<Eigen::Vector3f> nghbr_point (
static_cast<float*
> (data_n));
666 Eigen::Map<Eigen::Vector3f> initial_point (
static_cast<float*
> (data_p));
667 Eigen::Map<Eigen::Vector3f> initial_normal (
static_cast<float*
> ((*normals_)[point].normal));
668 float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
669 if (residual > residual_threshold_)
677 template <
typename Po
intT,
typename NormalT>
void
682 bool segmentation_is_possible = initCompute ();
683 if ( !segmentation_is_possible )
690 bool point_was_found =
false;
691 for (
const auto& point : (*indices_))
694 point_was_found =
true;
700 if (clusters_.empty ())
703 point_neighbours_.clear ();
704 point_labels_.clear ();
705 num_pts_in_segment_.clear ();
706 point_distances_.clear ();
707 segment_neighbours_.clear ();
708 segment_distances_.clear ();
709 segment_labels_.clear ();
710 number_of_segments_ = 0;
712 segmentation_is_possible = prepareForSegmentation ();
713 if ( !segmentation_is_possible )
719 findPointNeighbours ();
720 applySmoothRegionGrowingAlgorithm ();
723 findSegmentNeighbours ();
724 applyRegionMergingAlgorithm ();
728 for (
const auto& i_segment : clusters_)
730 const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
731 if (it != i_segment.indices.cend())
735 cluster.
indices.reserve (i_segment.indices.size ());
736 std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.
indices));
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
bool normal_flag_
If set to true then normal/smoothness test will be done during segmentation.
bool residual_flag_
If set to true then residual test will be done during segmentation.
pcl::uindex_t min_pts_per_cluster_
Stores the minimum number of points that a cluster needs to contain in order to be considered valid.
void assembleRegions()
This function simply assembles the regions from list of point labels.
void getSegmentFromPoint(index_t index, pcl::PointIndices &cluster) override
For a given point this function builds a segment to which it belongs and returns this segment.
void setDistanceThreshold(float thresh)
Allows to set distance threshold.
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
bool prepareForSegmentation() override
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
void findRegionNeighbours(std::vector< std::vector< std::pair< float, pcl::index_t > > > &neighbours_out, std::vector< std::vector< int > > ®ions_in)
This method assembles the array containing neighbours of each homogeneous region.
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
void setCurvatureTestFlag(bool value) override
Allows to turn on/off the curvature test.
void extract(std::vector< pcl::PointIndices > &clusters) override
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
void setNormalTestFlag(bool value)
Allows to turn on/off the smoothness test.
void setNumberOfRegionNeighbours(unsigned int nghbr_number)
This method allows to set the number of neighbours that is used for finding neighbouring segments.
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.
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
void findRegionsKNN(pcl::index_t index, pcl::uindex_t nghbr_number, Indices &nghbrs, std::vector< float > &dist)
This method finds K nearest neighbours of the given segment.
RegionGrowingRGB()
Constructor that sets default values for member variables.
void setResidualTestFlag(bool value) override
Allows to turn on/off the residual test.
~RegionGrowingRGB() override
Destructor that frees memory.
void findPointNeighbours() override
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
float getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use.
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.
bool validatePoint(index_t initial_seed, index_t point, index_t nghbr, bool &is_a_seed) const override
This function is checking if the point with index 'nghbr' belongs to the segment.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.