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_