164 point_neighbours_.clear ();
165 point_labels_.clear ();
166 num_pts_in_segment_.clear ();
167 point_distances_.clear ();
168 segment_neighbours_.clear ();
169 segment_distances_.clear ();
170 segment_labels_.clear ();
171 number_of_segments_ = 0;
173 bool segmentation_is_possible = initCompute ();
174 if ( !segmentation_is_possible )
180 segmentation_is_possible = prepareForSegmentation ();
181 if ( !segmentation_is_possible )
187 findPointNeighbours ();
188 applySmoothRegionGrowingAlgorithm ();
191 findSegmentNeighbours ();
192 applyRegionMergingAlgorithm ();
194 auto cluster_iter = clusters_.begin ();
195 while (cluster_iter != clusters_.end ())
197 if (cluster_iter->indices.size () < min_pts_per_cluster_ ||
198 cluster_iter->indices.size () > max_pts_per_cluster_)
200 cluster_iter = clusters_.erase (cluster_iter);
206 clusters.reserve (clusters_.size ());
207 std::copy (clusters_.cbegin (), clusters_.cend (), std::back_inserter (clusters));
310 std::vector<float> distances;
311 float max_dist = std::numeric_limits<float>::max ();
312 distances.resize (clusters_.size (), max_dist);
314 const auto number_of_points = num_pts_in_segment_[index];
316 for (
pcl::uindex_t i_point = 0; i_point < number_of_points; i_point++)
318 const auto point_index = clusters_[index].indices[i_point];
319 const auto number_of_neighbours = point_neighbours_[point_index].size ();
322 for (std::size_t i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
325 const pcl::index_t segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
327 if ( segment_index != index )
330 if (distances[segment_index] > point_distances_[point_index][i_nghbr])
331 distances[segment_index] = point_distances_[point_index][i_nghbr];
336 std::priority_queue<std::pair<float, int> > segment_neighbours;
337 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
339 if (distances[i_seg] < max_dist)
341 segment_neighbours.emplace (distances[i_seg], i_seg);
342 if (segment_neighbours.size () > nghbr_number)
343 segment_neighbours.pop ();
347 const std::size_t size = std::min<std::size_t> (segment_neighbours.size (),
static_cast<std::size_t
>(nghbr_number));
348 nghbrs.resize (size, 0);
349 dist.resize (size, 0);
351 while ( !segment_neighbours.empty () && counter < nghbr_number )
353 dist[counter] = segment_neighbours.top ().first;
354 nghbrs[counter] = segment_neighbours.top ().second;
355 segment_neighbours.pop ();
365 std::vector< std::vector<unsigned int> > segment_color;
366 std::vector<unsigned int> color;
368 segment_color.resize (number_of_segments_, color);
370 for (
const auto& point_index : (*indices_))
372 int segment_index = point_labels_[point_index];
373 segment_color[segment_index][0] += (*input_)[point_index].r;
374 segment_color[segment_index][1] += (*input_)[point_index].g;
375 segment_color[segment_index][2] += (*input_)[point_index].b;
377 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
379 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]));
380 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]));
381 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]));
386 std::vector<unsigned int> num_pts_in_homogeneous_region;
387 std::vector<int> num_seg_in_homogeneous_region;
389 segment_labels_.resize (number_of_segments_, -1);
391 float dist_thresh = distance_threshold_;
392 int homogeneous_region_number = 0;
393 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
395 int curr_homogeneous_region = 0;
396 if (segment_labels_[i_seg] == -1)
398 segment_labels_[i_seg] = homogeneous_region_number;
399 curr_homogeneous_region = homogeneous_region_number;
400 num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
401 num_seg_in_homogeneous_region.push_back (1);
402 homogeneous_region_number++;
405 curr_homogeneous_region = segment_labels_[i_seg];
407 unsigned int i_nghbr = 0;
408 while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
410 int index = segment_neighbours_[i_seg][i_nghbr];
411 if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
416 if ( segment_labels_[index] == -1 )
418 float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
419 if (difference < color_r2r_threshold_)
421 segment_labels_[index] = curr_homogeneous_region;
422 num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
423 num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
430 segment_color.clear ();
433 std::vector< std::vector<int> > final_segments;
434 std::vector<int> region;
435 final_segments.resize (homogeneous_region_number, region);
436 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
438 final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
441 std::vector<int> counter;
442 counter.resize (homogeneous_region_number, 0);
443 for (
int i_seg = 0; i_seg < number_of_segments_; i_seg++)
445 int index = segment_labels_[i_seg];
446 final_segments[ index ][ counter[index] ] = i_seg;
450 std::vector< std::vector< std::pair<float, pcl::index_t> > > region_neighbours;
451 findRegionNeighbours (region_neighbours, final_segments);
453 int final_segment_number = homogeneous_region_number;
454 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
456 if (num_pts_in_homogeneous_region[i_reg] < min_pts_per_cluster_)
458 if ( region_neighbours[i_reg].empty () )
460 int nearest_neighbour = region_neighbours[i_reg][0].second;
461 if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
463 int reg_index = segment_labels_[nearest_neighbour];
464 int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
465 for (
int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
467 int segment_index = final_segments[i_reg][i_seg];
468 final_segments[reg_index].push_back (segment_index);
469 segment_labels_[segment_index] = reg_index;
471 final_segments[i_reg].clear ();
472 num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
473 num_pts_in_homogeneous_region[i_reg] = 0;
474 num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
475 num_seg_in_homogeneous_region[i_reg] = 0;
476 final_segment_number -= 1;
478 for (
auto& nghbr : region_neighbours[reg_index])
480 if ( segment_labels_[ nghbr.second ] == reg_index )
482 nghbr.first = std::numeric_limits<float>::max ();
486 for (
const auto& nghbr : region_neighbours[i_reg])
488 if ( segment_labels_[ nghbr.second ] != reg_index )
490 region_neighbours[reg_index].push_back (nghbr);
493 region_neighbours[i_reg].clear ();
494 std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (),
comparePair);
498 assembleRegions (num_pts_in_homogeneous_region,
static_cast<int> (num_pts_in_homogeneous_region.size ()));
500 number_of_segments_ = final_segment_number;
518 int region_number =
static_cast<int> (regions_in.size ());
519 neighbours_out.clear ();
520 neighbours_out.resize (region_number);
522 for (
int i_reg = 0; i_reg < region_number; i_reg++)
524 neighbours_out[i_reg].reserve (regions_in[i_reg].size () * region_neighbour_number_);
525 for (
const auto& curr_segment : regions_in[i_reg])
527 const std::size_t nghbr_number = segment_neighbours_[curr_segment].size ();
528 std::pair<float, pcl::index_t> pair;
529 for (std::size_t i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
531 const auto segment_index = segment_neighbours_[curr_segment][i_nghbr];
532 if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
534 if (segment_labels_[segment_index] != i_reg)
536 pair.first = segment_distances_[curr_segment][i_nghbr];
537 pair.second = segment_index;
538 neighbours_out[i_reg].push_back (pair);
542 std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (),
comparePair);
552 clusters_.resize (num_regions, segment);
553 for (
int i_seg = 0; i_seg < num_regions; i_seg++)
555 clusters_[i_seg].
indices.resize (num_pts_in_region[i_seg]);
558 std::vector<int> counter;
559 counter.resize (num_regions, 0);
560 for (
const auto& point_index : (*indices_))
562 int index = point_labels_[point_index];
563 index = segment_labels_[index];
564 clusters_[index].indices[ counter[index] ] = point_index;
569 if (clusters_.empty ())
572 std::vector<pcl::PointIndices>::iterator itr1, itr2;
573 itr1 = clusters_.begin ();
574 itr2 = clusters_.end () - 1;
578 while (!(itr1->indices.empty ()) && itr1 < itr2)
580 while ( itr2->indices.empty () && itr1 < itr2)
584 itr1->indices.swap (itr2->indices);
587 if (itr2->indices.empty ())
588 clusters_.erase (itr2, clusters_.end ());
598 std::vector<unsigned int> point_color;
599 point_color.resize (3, 0);
600 std::vector<unsigned int> nghbr_color;
601 nghbr_color.resize (3, 0);
602 point_color[0] = (*input_)[point].r;
603 point_color[1] = (*input_)[point].g;
604 point_color[2] = (*input_)[point].b;
605 nghbr_color[0] = (*input_)[nghbr].r;
606 nghbr_color[1] = (*input_)[nghbr].g;
607 nghbr_color[2] = (*input_)[nghbr].b;
608 float difference = calculateColorimetricalDifference (point_color, nghbr_color);
609 if (difference > color_p2p_threshold_)
612 float cosine_threshold = std::cos (theta_threshold_);
618 data[0] = (*input_)[point].data[0];
619 data[1] = (*input_)[point].data[1];
620 data[2] = (*input_)[point].data[2];
621 data[3] = (*input_)[point].data[3];
623 Eigen::Map<Eigen::Vector3f> initial_point (
static_cast<float*
> (data));
624 Eigen::Map<Eigen::Vector3f> initial_normal (
static_cast<float*
> ((*normals_)[point].normal));
625 if (smooth_mode_flag_ ==
true)
627 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
628 float dot_product = std::abs (nghbr_normal.dot (initial_normal));
629 if (dot_product < cosine_threshold)
634 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
635 Eigen::Map<Eigen::Vector3f> initial_seed_normal (
static_cast<float*
> ((*normals_)[initial_seed].normal));
636 float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
637 if (dot_product < cosine_threshold)
643 if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
650 data_p[0] = (*input_)[point].data[0];
651 data_p[1] = (*input_)[point].data[1];
652 data_p[2] = (*input_)[point].data[2];
653 data_p[3] = (*input_)[point].data[3];
655 data_n[0] = (*input_)[nghbr].data[0];
656 data_n[1] = (*input_)[nghbr].data[1];
657 data_n[2] = (*input_)[nghbr].data[2];
658 data_n[3] = (*input_)[nghbr].data[3];
659 Eigen::Map<Eigen::Vector3f> nghbr_point (
static_cast<float*
> (data_n));
660 Eigen::Map<Eigen::Vector3f> initial_point (
static_cast<float*
> (data_p));
661 Eigen::Map<Eigen::Vector3f> initial_normal (
static_cast<float*
> ((*normals_)[point].normal));
662 float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
663 if (residual > residual_threshold_)
676 bool segmentation_is_possible = initCompute ();
677 if ( !segmentation_is_possible )
684 bool point_was_found =
false;
685 for (
const auto& point : (*indices_))
688 point_was_found =
true;
694 if (clusters_.empty ())
697 point_neighbours_.clear ();
698 point_labels_.clear ();
699 num_pts_in_segment_.clear ();
700 point_distances_.clear ();
701 segment_neighbours_.clear ();
702 segment_distances_.clear ();
703 segment_labels_.clear ();
704 number_of_segments_ = 0;
706 segmentation_is_possible = prepareForSegmentation ();
707 if ( !segmentation_is_possible )
713 findPointNeighbours ();
714 applySmoothRegionGrowingAlgorithm ();
717 findSegmentNeighbours ();
718 applyRegionMergingAlgorithm ();
722 for (
const auto& i_segment : clusters_)
724 const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
725 if (it != i_segment.indices.cend())
729 cluster.
indices.reserve (i_segment.indices.size ());
730 std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.
indices));