42 #include <pcl/segmentation/region_growing.h>
44 #include <pcl/point_cloud.h>
46 #include <pcl/common/point_tests.h>
47 #include <pcl/console/print.h>
48 #include <pcl/search/search.h>
49 #include <pcl/search/kdtree.h>
56 template <
typename Po
intT,
typename NormalT>
58 min_pts_per_cluster_ (1),
59 max_pts_per_cluster_ (std::numeric_limits<int>::max ()),
60 smooth_mode_flag_ (true),
61 curvature_flag_ (true),
62 residual_flag_ (false),
63 theta_threshold_ (30.0f / 180.0f * static_cast<float> (
M_PI)),
64 residual_threshold_ (0.05f),
65 curvature_threshold_ (0.05f),
66 neighbour_number_ (30),
69 point_neighbours_ (0),
72 num_pts_in_segment_ (0),
74 number_of_segments_ (0)
79 template <
typename Po
intT,
typename NormalT>
82 point_neighbours_.clear ();
83 point_labels_.clear ();
84 num_pts_in_segment_.clear ();
89 template <
typename Po
intT,
typename NormalT>
int
92 return (min_pts_per_cluster_);
96 template <
typename Po
intT,
typename NormalT>
void
99 min_pts_per_cluster_ = min_cluster_size;
103 template <
typename Po
intT,
typename NormalT>
int
106 return (max_pts_per_cluster_);
110 template <
typename Po
intT,
typename NormalT>
void
113 max_pts_per_cluster_ = max_cluster_size;
117 template <
typename Po
intT,
typename NormalT>
bool
120 return (smooth_mode_flag_);
124 template <
typename Po
intT,
typename NormalT>
void
127 smooth_mode_flag_ = value;
131 template <
typename Po
intT,
typename NormalT>
bool
134 return (curvature_flag_);
138 template <
typename Po
intT,
typename NormalT>
void
141 curvature_flag_ = value;
143 if (curvature_flag_ ==
false && residual_flag_ ==
false)
144 residual_flag_ =
true;
148 template <
typename Po
intT,
typename NormalT>
bool
151 return (residual_flag_);
155 template <
typename Po
intT,
typename NormalT>
void
158 residual_flag_ = value;
160 if (curvature_flag_ ==
false && residual_flag_ ==
false)
161 curvature_flag_ =
true;
165 template <
typename Po
intT,
typename NormalT>
float
168 return (theta_threshold_);
172 template <
typename Po
intT,
typename NormalT>
void
175 theta_threshold_ = theta;
179 template <
typename Po
intT,
typename NormalT>
float
182 return (residual_threshold_);
186 template <
typename Po
intT,
typename NormalT>
void
189 residual_threshold_ = residual;
193 template <
typename Po
intT,
typename NormalT>
float
196 return (curvature_threshold_);
200 template <
typename Po
intT,
typename NormalT>
void
203 curvature_threshold_ = curvature;
207 template <
typename Po
intT,
typename NormalT>
unsigned int
210 return (neighbour_number_);
214 template <
typename Po
intT,
typename NormalT>
void
217 neighbour_number_ = neighbour_number;
228 template <
typename Po
intT,
typename NormalT>
void
242 template <
typename Po
intT,
typename NormalT>
void
249 template <
typename Po
intT,
typename NormalT>
void
254 point_neighbours_.clear ();
255 point_labels_.clear ();
256 num_pts_in_segment_.clear ();
257 number_of_segments_ = 0;
259 bool segmentation_is_possible = initCompute ();
260 if ( !segmentation_is_possible )
266 segmentation_is_possible = prepareForSegmentation ();
267 if ( !segmentation_is_possible )
273 findPointNeighbours ();
274 applySmoothRegionGrowingAlgorithm ();
277 clusters.resize (clusters_.size ());
278 std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin ();
279 for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); ++cluster_iter)
281 if ((
static_cast<int> (cluster_iter->indices.size ()) >= min_pts_per_cluster_) &&
282 (
static_cast<int> (cluster_iter->indices.size ()) <= max_pts_per_cluster_))
284 *cluster_iter_input = *cluster_iter;
285 ++cluster_iter_input;
289 clusters_ = std::vector<pcl::PointIndices> (clusters.begin (), cluster_iter_input);
290 clusters.resize(clusters_.size());
296 template <
typename Po
intT,
typename NormalT>
bool
300 if ( input_->points.empty () )
304 if ( !normals_ || input_->size () != normals_->size () )
310 if (residual_threshold_ <= 0.0f)
322 if (neighbour_number_ == 0)
331 if (indices_->empty ())
332 PCL_ERROR (
"[pcl::RegionGrowing::prepareForSegmentation] Empty given indices!\n");
333 search_->setInputCloud (input_, indices_);
336 search_->setInputCloud (input_);
342 template <
typename Po
intT,
typename NormalT>
void
345 int point_number =
static_cast<int> (indices_->size ());
346 std::vector<int> neighbours;
347 std::vector<float> distances;
349 point_neighbours_.resize (input_->size (), neighbours);
350 if (input_->is_dense)
352 for (
int i_point = 0; i_point < point_number; i_point++)
354 int point_index = (*indices_)[i_point];
356 search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
357 point_neighbours_[point_index].swap (neighbours);
362 for (
int i_point = 0; i_point < point_number; i_point++)
365 int point_index = (*indices_)[i_point];
368 search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
369 point_neighbours_[point_index].swap (neighbours);
375 template <
typename Po
intT,
typename NormalT>
void
378 int num_of_pts =
static_cast<int> (indices_->size ());
379 point_labels_.resize (input_->size (), -1);
381 std::vector< std::pair<float, int> > point_residual;
382 std::pair<float, int> pair;
383 point_residual.resize (num_of_pts, pair);
385 if (normal_flag_ ==
true)
387 for (
int i_point = 0; i_point < num_of_pts; i_point++)
389 int point_index = (*indices_)[i_point];
390 point_residual[i_point].first = (*normals_)[point_index].curvature;
391 point_residual[i_point].second = point_index;
393 std::sort (point_residual.begin (), point_residual.end (),
comparePair);
397 for (
int i_point = 0; i_point < num_of_pts; i_point++)
399 int point_index = (*indices_)[i_point];
400 point_residual[i_point].first = 0;
401 point_residual[i_point].second = point_index;
404 int seed_counter = 0;
405 int seed = point_residual[seed_counter].second;
407 int segmented_pts_num = 0;
408 int number_of_segments = 0;
409 while (segmented_pts_num < num_of_pts)
412 pts_in_segment = growRegion (seed, number_of_segments);
413 segmented_pts_num += pts_in_segment;
414 num_pts_in_segment_.push_back (pts_in_segment);
415 number_of_segments++;
418 for (
int i_seed = seed_counter + 1; i_seed < num_of_pts; i_seed++)
420 int index = point_residual[i_seed].second;
421 if (point_labels_[index] == -1)
424 seed_counter = i_seed;
432 template <
typename Po
intT,
typename NormalT>
int
435 std::queue<int> seeds;
436 seeds.push (initial_seed);
437 point_labels_[initial_seed] = segment_number;
439 int num_pts_in_segment = 1;
441 while (!seeds.empty ())
444 curr_seed = seeds.front ();
447 std::size_t i_nghbr = 0;
448 while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () )
450 int index = point_neighbours_[curr_seed][i_nghbr];
451 if (point_labels_[index] != -1)
457 bool is_a_seed =
false;
458 bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed);
460 if (!belongs_to_segment)
466 point_labels_[index] = segment_number;
467 num_pts_in_segment++;
478 return (num_pts_in_segment);
482 template <
typename Po
intT,
typename NormalT>
bool
487 float cosine_threshold = std::cos (theta_threshold_);
490 data[0] = (*input_)[point].data[0];
491 data[1] = (*input_)[point].data[1];
492 data[2] = (*input_)[point].data[2];
493 data[3] = (*input_)[point].data[3];
494 Eigen::Map<Eigen::Vector3f> initial_point (
static_cast<float*
> (data));
495 Eigen::Map<Eigen::Vector3f> initial_normal (
static_cast<float*
> ((*normals_)[point].normal));
498 if (smooth_mode_flag_ ==
true)
500 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
501 float dot_product = std::abs (nghbr_normal.dot (initial_normal));
502 if (dot_product < cosine_threshold)
509 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
510 Eigen::Map<Eigen::Vector3f> initial_seed_normal (
static_cast<float*
> ((*normals_)[initial_seed].normal));
511 float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
512 if (dot_product < cosine_threshold)
517 if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
525 data_1[0] = (*input_)[nghbr].data[0];
526 data_1[1] = (*input_)[nghbr].data[1];
527 data_1[2] = (*input_)[nghbr].data[2];
528 data_1[3] = (*input_)[nghbr].data[3];
529 Eigen::Map<Eigen::Vector3f> nghbr_point (
static_cast<float*
> (data_1));
530 float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
531 if (residual_flag_ && residual > residual_threshold_)
538 template <
typename Po
intT,
typename NormalT>
void
541 const auto number_of_segments = num_pts_in_segment_.size ();
542 const auto number_of_points = input_->size ();
545 clusters_.resize (number_of_segments, segment);
547 for (std::size_t i_seg = 0; i_seg < number_of_segments; i_seg++)
549 clusters_[i_seg].
indices.resize ( num_pts_in_segment_[i_seg], 0);
552 std::vector<int> counter(number_of_segments, 0);
554 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
556 const auto segment_index = point_labels_[i_point];
557 if (segment_index != -1)
559 const auto point_index = counter[segment_index];
560 clusters_[segment_index].indices[point_index] = i_point;
561 counter[segment_index] = point_index + 1;
565 number_of_segments_ = number_of_segments;
569 template <
typename Po
intT,
typename NormalT>
void
574 bool segmentation_is_possible = initCompute ();
575 if ( !segmentation_is_possible )
582 bool point_was_found =
false;
583 int number_of_points =
static_cast <int> (indices_->size ());
584 for (
int point = 0; point < number_of_points; point++)
585 if ( (*indices_)[point] == index)
587 point_was_found =
true;
593 if (clusters_.empty ())
595 point_neighbours_.clear ();
596 point_labels_.clear ();
597 num_pts_in_segment_.clear ();
598 number_of_segments_ = 0;
600 segmentation_is_possible = prepareForSegmentation ();
601 if ( !segmentation_is_possible )
607 findPointNeighbours ();
608 applySmoothRegionGrowingAlgorithm ();
613 for (
auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
615 bool segment_was_found =
false;
616 for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
618 if (i_segment->indices[i_point] == index)
620 segment_was_found =
true;
622 cluster.
indices.reserve (i_segment->indices.size ());
623 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
627 if (segment_was_found)
643 if (!clusters_.empty ())
647 srand (
static_cast<unsigned int> (time (
nullptr)));
648 std::vector<unsigned char> colors;
649 for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
651 colors.push_back (
static_cast<unsigned char> (rand () % 256));
652 colors.push_back (
static_cast<unsigned char> (rand () % 256));
653 colors.push_back (
static_cast<unsigned char> (rand () % 256));
656 colored_cloud->
width = input_->width;
657 colored_cloud->
height = input_->height;
658 colored_cloud->
is_dense = input_->is_dense;
659 for (
const auto& i_point: *input_)
662 point.x = *(i_point.data);
663 point.y = *(i_point.data + 1);
664 point.z = *(i_point.data + 2);
668 colored_cloud->
points.push_back (point);
672 for (
auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
674 for (
auto i_point = i_segment->indices.cbegin (); i_point != i_segment->indices.cend (); i_point++)
678 (*colored_cloud)[index].r = colors[3 * next_color];
679 (*colored_cloud)[index].g = colors[3 * next_color + 1];
680 (*colored_cloud)[index].b = colors[3 * next_color + 2];
686 return (colored_cloud);
695 if (!clusters_.empty ())
699 srand (
static_cast<unsigned int> (time (
nullptr)));
700 std::vector<unsigned char> colors;
701 for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
703 colors.push_back (
static_cast<unsigned char> (rand () % 256));
704 colors.push_back (
static_cast<unsigned char> (rand () % 256));
705 colors.push_back (
static_cast<unsigned char> (rand () % 256));
708 colored_cloud->
width = input_->width;
709 colored_cloud->
height = input_->height;
710 colored_cloud->
is_dense = input_->is_dense;
711 for (
const auto& i_point: *input_)
714 point.x = *(i_point.data);
715 point.y = *(i_point.data + 1);
716 point.z = *(i_point.data + 2);
721 colored_cloud->
points.push_back (point);
725 for (
auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
727 for (
auto i_point = i_segment->indices.cbegin (); i_point != i_segment->indices.cend (); i_point++)
729 int index = *i_point;
730 (*colored_cloud)[index].r = colors[3 * next_color];
731 (*colored_cloud)[index].g = colors[3 * next_color + 1];
732 (*colored_cloud)[index].b = colors[3 * next_color + 2];
738 return (colored_cloud);
741 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>;