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 std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin ();
199 while (cluster_iter != clusters_.end ())
201 if (
static_cast<int> (cluster_iter->indices.size ()) < min_pts_per_cluster_ ||
202 static_cast<int> (cluster_iter->indices.size ()) > max_pts_per_cluster_)
204 cluster_iter = clusters_.erase (cluster_iter);
210 clusters.reserve (clusters_.size ());
211 std::copy (clusters_.begin (), clusters_.end (), 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 ());
276 std::vector<int> neighbours;
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
297 std::vector<int> neighbours;
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++)
304 std::vector<int> nghbrs;
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 int number_of_points = num_pts_in_segment_[index];
322 for (
int i_point = 0; i_point < number_of_points; i_point++)
324 int point_index = clusters_[index].indices[i_point];
325 int number_of_neighbours =
static_cast<int> (point_neighbours_[point_index].size ());
328 for (
int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
331 int segment_index = -1;
332 segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
334 if ( segment_index != index )
337 if (distances[segment_index] > point_distances_[point_index][i_nghbr])
338 distances[segment_index] = point_distances_[point_index][i_nghbr];
343 std::priority_queue<std::pair<float, int> > segment_neighbours;
344 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
346 if (distances[i_seg] < max_dist)
348 segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) );
349 if (
int (segment_neighbours.size ()) > nghbr_number)
350 segment_neighbours.pop ();
354 int size = std::min<int> (
static_cast<int> (segment_neighbours.size ()), nghbr_number);
355 nghbrs.resize (size, 0);
356 dist.resize (size, 0);
358 while ( !segment_neighbours.empty () && counter < nghbr_number )
360 dist[counter] = segment_neighbours.top ().first;
361 nghbrs[counter] = segment_neighbours.top ().second;
362 segment_neighbours.pop ();
368 template <
typename Po
intT,
typename NormalT>
void
371 int number_of_points =
static_cast<int> (indices_->size ());
374 std::vector< std::vector<unsigned int> > segment_color;
375 std::vector<unsigned int> color;
377 segment_color.resize (number_of_segments_, color);
379 for (
int i_point = 0; i_point < number_of_points; i_point++)
381 int point_index = (*indices_)[i_point];
382 int segment_index = point_labels_[point_index];
383 segment_color[segment_index][0] += (*input_)[point_index].r;
384 segment_color[segment_index][1] += (*input_)[point_index].g;
385 segment_color[segment_index][2] += (*input_)[point_index].b;
387 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
389 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]));
390 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]));
391 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]));
396 std::vector<unsigned int> num_pts_in_homogeneous_region;
397 std::vector<int> num_seg_in_homogeneous_region;
399 segment_labels_.resize (number_of_segments_, -1);
401 float dist_thresh = distance_threshold_;
402 int homogeneous_region_number = 0;
403 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
405 int 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_)[point].r;
622 point_color[1] = (*input_)[point].g;
623 point_color[2] = (*input_)[point].b;
624 nghbr_color[0] = (*input_)[nghbr].r;
625 nghbr_color[1] = (*input_)[nghbr].g;
626 nghbr_color[2] = (*input_)[nghbr].b;
627 float difference = calculateColorimetricalDifference (point_color, nghbr_color);
628 if (difference > color_p2p_threshold_)
631 float cosine_threshold = std::cos (theta_threshold_);
637 data[0] = (*input_)[point].data[0];
638 data[1] = (*input_)[point].data[1];
639 data[2] = (*input_)[point].data[2];
640 data[3] = (*input_)[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_)[point].normal));
644 if (smooth_mode_flag_ ==
true)
646 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
647 float dot_product = std::abs (nghbr_normal.dot (initial_normal));
648 if (dot_product < cosine_threshold)
653 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
654 Eigen::Map<Eigen::Vector3f> initial_seed_normal (
static_cast<float*
> ((*normals_)[initial_seed].normal));
655 float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
656 if (dot_product < cosine_threshold)
662 if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
669 data_p[0] = (*input_)[point].data[0];
670 data_p[1] = (*input_)[point].data[1];
671 data_p[2] = (*input_)[point].data[2];
672 data_p[3] = (*input_)[point].data[3];
674 data_n[0] = (*input_)[nghbr].data[0];
675 data_n[1] = (*input_)[nghbr].data[1];
676 data_n[2] = (*input_)[nghbr].data[2];
677 data_n[3] = (*input_)[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_)[point].normal));
681 float residual = std::abs (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 for (
auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
744 bool segment_was_found =
false;
745 for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
747 if (i_segment->indices[i_point] == index)
749 segment_was_found =
true;
751 cluster.
indices.reserve (i_segment->indices.size ());
752 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
756 if (segment_was_found)
766 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_