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 (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_.begin (), clusters_.end (), std::back_inserter (clusters));
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 ();
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;
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);
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 ());
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_)
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));