40 #ifndef PCL_SEGMENTATION_REGION_GROWING_HPP_
41 #define PCL_SEGMENTATION_REGION_GROWING_HPP_
43 #include <pcl/segmentation/region_growing.h>
45 #include <pcl/search/search.h>
46 #include <pcl/search/kdtree.h>
47 #include <pcl/point_cloud.h>
48 #include <pcl/point_types.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>
87 point_neighbours_.clear ();
88 point_labels_.clear ();
89 num_pts_in_segment_.clear ();
94 template <
typename Po
intT,
typename NormalT>
int
97 return (min_pts_per_cluster_);
101 template <
typename Po
intT,
typename NormalT>
void
104 min_pts_per_cluster_ = min_cluster_size;
108 template <
typename Po
intT,
typename NormalT>
int
111 return (max_pts_per_cluster_);
115 template <
typename Po
intT,
typename NormalT>
void
118 max_pts_per_cluster_ = max_cluster_size;
122 template <
typename Po
intT,
typename NormalT>
bool
125 return (smooth_mode_flag_);
129 template <
typename Po
intT,
typename NormalT>
void
132 smooth_mode_flag_ = value;
136 template <
typename Po
intT,
typename NormalT>
bool
139 return (curvature_flag_);
143 template <
typename Po
intT,
typename NormalT>
void
146 curvature_flag_ = value;
148 if (curvature_flag_ ==
false && residual_flag_ ==
false)
149 residual_flag_ =
true;
153 template <
typename Po
intT,
typename NormalT>
bool
156 return (residual_flag_);
160 template <
typename Po
intT,
typename NormalT>
void
163 residual_flag_ = value;
165 if (curvature_flag_ ==
false && residual_flag_ ==
false)
166 curvature_flag_ =
true;
170 template <
typename Po
intT,
typename NormalT>
float
173 return (theta_threshold_);
177 template <
typename Po
intT,
typename NormalT>
void
180 theta_threshold_ = theta;
184 template <
typename Po
intT,
typename NormalT>
float
187 return (residual_threshold_);
191 template <
typename Po
intT,
typename NormalT>
void
194 residual_threshold_ = residual;
198 template <
typename Po
intT,
typename NormalT>
float
201 return (curvature_threshold_);
205 template <
typename Po
intT,
typename NormalT>
void
208 curvature_threshold_ = curvature;
212 template <
typename Po
intT,
typename NormalT>
unsigned int
215 return (neighbour_number_);
219 template <
typename Po
intT,
typename NormalT>
void
222 neighbour_number_ = neighbour_number;
233 template <
typename Po
intT,
typename NormalT>
void
250 template <
typename Po
intT,
typename NormalT>
void
260 template <
typename Po
intT,
typename NormalT>
void
265 point_neighbours_.clear ();
266 point_labels_.clear ();
267 num_pts_in_segment_.clear ();
268 number_of_segments_ = 0;
270 bool segmentation_is_possible = initCompute ();
271 if ( !segmentation_is_possible )
277 segmentation_is_possible = prepareForSegmentation ();
278 if ( !segmentation_is_possible )
284 findPointNeighbours ();
285 applySmoothRegionGrowingAlgorithm ();
288 clusters.resize (clusters_.size ());
289 std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin ();
290 for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); cluster_iter++)
292 if ((cluster_iter->indices.size () >= min_pts_per_cluster_) &&
293 (cluster_iter->indices.size () <= max_pts_per_cluster_))
295 *cluster_iter_input = *cluster_iter;
296 cluster_iter_input++;
300 clusters_ = std::vector<pcl::PointIndices> (clusters.begin (), cluster_iter_input);
301 clusters.resize(clusters_.size());
307 template <
typename Po
intT,
typename NormalT>
bool
311 if ( input_->points.size () == 0 )
315 if ( normals_ == 0 || input_->points.size () != normals_->points.size () )
321 if (residual_threshold_ <= 0.0f)
333 if (neighbour_number_ == 0)
342 if (indices_->empty ())
343 PCL_ERROR (
"[pcl::RegionGrowing::prepareForSegmentation] Empty given indices!\n");
344 search_->setInputCloud (input_, indices_);
347 search_->setInputCloud (input_);
353 template <
typename Po
intT,
typename NormalT>
void
356 int point_number =
static_cast<int> (indices_->size ());
357 std::vector<int> neighbours;
358 std::vector<float> distances;
360 point_neighbours_.resize (input_->points.size (), neighbours);
362 for (
int i_point = 0; i_point < point_number; i_point++)
364 int point_index = (*indices_)[i_point];
366 search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
367 point_neighbours_[point_index].swap (neighbours);
372 template <
typename Po
intT,
typename NormalT>
void
375 int num_of_pts =
static_cast<int> (indices_->size ());
376 point_labels_.resize (input_->points.size (), -1);
378 std::vector< std::pair<float, int> > point_residual;
379 std::pair<float, int> pair;
380 point_residual.resize (num_of_pts, pair);
382 if (normal_flag_ ==
true)
384 for (
int i_point = 0; i_point < num_of_pts; i_point++)
386 int point_index = (*indices_)[i_point];
387 point_residual[i_point].first = normals_->points[point_index].curvature;
388 point_residual[i_point].second = point_index;
390 std::sort (point_residual.begin (), point_residual.end (),
comparePair);
394 for (
int i_point = 0; i_point < num_of_pts; i_point++)
396 int point_index = (*indices_)[i_point];
397 point_residual[i_point].first = 0;
398 point_residual[i_point].second = point_index;
401 int seed_counter = 0;
402 int seed = point_residual[seed_counter].second;
404 int segmented_pts_num = 0;
405 int number_of_segments = 0;
406 while (segmented_pts_num < num_of_pts)
409 pts_in_segment = growRegion (seed, number_of_segments);
410 segmented_pts_num += pts_in_segment;
411 num_pts_in_segment_.push_back (pts_in_segment);
412 number_of_segments++;
415 for (
int i_seed = seed_counter + 1; i_seed < num_of_pts; i_seed++)
417 int index = point_residual[i_seed].second;
418 if (point_labels_[index] == -1)
428 template <
typename Po
intT,
typename NormalT>
int
431 std::queue<int> seeds;
432 seeds.push (initial_seed);
433 point_labels_[initial_seed] = segment_number;
435 int num_pts_in_segment = 1;
437 while (!seeds.empty ())
440 curr_seed = seeds.front ();
444 while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () )
446 int index = point_neighbours_[curr_seed][i_nghbr];
447 if (point_labels_[index] != -1)
453 bool is_a_seed =
false;
454 bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed);
456 if (belongs_to_segment ==
false)
462 point_labels_[index] = segment_number;
463 num_pts_in_segment++;
474 return (num_pts_in_segment);
478 template <
typename Po
intT,
typename NormalT>
bool
483 float cosine_threshold = cosf (theta_threshold_);
486 data[0] = input_->points[point].data[0];
487 data[1] = input_->points[point].data[1];
488 data[2] = input_->points[point].data[2];
489 data[3] = input_->points[point].data[3];
490 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
491 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
494 if (smooth_mode_flag_ ==
true)
496 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
497 float dot_product = fabsf (nghbr_normal.dot (initial_normal));
498 if (dot_product < cosine_threshold)
505 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
506 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal));
507 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
508 if (dot_product < cosine_threshold)
513 if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
519 data[0] = input_->points[nghbr].data[0];
520 data[1] = input_->points[nghbr].data[1];
521 data[2] = input_->points[nghbr].data[2];
522 data[3] = input_->points[nghbr].data[3];
523 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data));
524 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
525 if (residual_flag_ && residual > residual_threshold_)
532 template <
typename Po
intT,
typename NormalT>
void
535 int number_of_segments =
static_cast<int> (num_pts_in_segment_.size ());
536 int number_of_points =
static_cast<int> (input_->points.size ());
539 clusters_.resize (number_of_segments, segment);
541 for (
int i_seg = 0; i_seg < number_of_segments; i_seg++)
543 clusters_[i_seg].
indices.resize ( num_pts_in_segment_[i_seg], 0);
546 std::vector<int> counter;
547 counter.resize (number_of_segments, 0);
549 for (
int i_point = 0; i_point < number_of_points; i_point++)
551 int segment_index = point_labels_[i_point];
552 if (segment_index != -1)
554 int point_index = counter[segment_index];
555 clusters_[segment_index].indices[point_index] = i_point;
556 counter[segment_index] = point_index + 1;
560 number_of_segments_ = number_of_segments;
564 template <
typename Po
intT,
typename NormalT>
void
569 bool segmentation_is_possible = initCompute ();
570 if ( !segmentation_is_possible )
577 bool point_was_found =
false;
578 int number_of_points = static_cast <
int> (indices_->size ());
579 for (
size_t point = 0; point < number_of_points; point++)
580 if ( (*indices_)[point] == index)
582 point_was_found =
true;
588 if (clusters_.empty ())
590 point_neighbours_.clear ();
591 point_labels_.clear ();
592 num_pts_in_segment_.clear ();
593 number_of_segments_ = 0;
595 segmentation_is_possible = prepareForSegmentation ();
596 if ( !segmentation_is_possible )
602 findPointNeighbours ();
603 applySmoothRegionGrowingAlgorithm ();
608 std::vector <pcl::PointIndices>::iterator i_segment;
609 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
611 bool segment_was_found =
false;
612 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
614 if (i_segment->indices[i_point] == index)
616 segment_was_found =
true;
618 cluster.
indices.reserve (i_segment->indices.size ());
619 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
623 if (segment_was_found)
639 if (!clusters_.empty ())
643 srand (static_cast<unsigned int> (time (0)));
644 std::vector<unsigned char> colors;
645 for (
size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
647 colors.push_back (static_cast<unsigned char> (rand () % 256));
648 colors.push_back (static_cast<unsigned char> (rand () % 256));
649 colors.push_back (static_cast<unsigned char> (rand () % 256));
652 colored_cloud->
width = input_->width;
653 colored_cloud->
height = input_->height;
654 colored_cloud->
is_dense = input_->is_dense;
655 for (
size_t i_point = 0; i_point < input_->points.size (); i_point++)
658 point.x = *(input_->points[i_point].data);
659 point.y = *(input_->points[i_point].data + 1);
660 point.z = *(input_->points[i_point].data + 2);
664 colored_cloud->
points.push_back (point);
667 std::vector< pcl::PointIndices >::iterator i_segment;
669 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
671 std::vector<int>::iterator i_point;
672 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
676 colored_cloud->
points[index].r = colors[3 * next_color];
677 colored_cloud->
points[index].g = colors[3 * next_color + 1];
678 colored_cloud->
points[index].b = colors[3 * next_color + 2];
684 return (colored_cloud);
693 if (!clusters_.empty ())
697 srand (static_cast<unsigned int> (time (0)));
698 std::vector<unsigned char> colors;
699 for (
size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
701 colors.push_back (static_cast<unsigned char> (rand () % 256));
702 colors.push_back (static_cast<unsigned char> (rand () % 256));
703 colors.push_back (static_cast<unsigned char> (rand () % 256));
706 colored_cloud->
width = input_->width;
707 colored_cloud->
height = input_->height;
708 colored_cloud->
is_dense = input_->is_dense;
709 for (
size_t i_point = 0; i_point < input_->points.size (); i_point++)
712 point.x = *(input_->points[i_point].data);
713 point.y = *(input_->points[i_point].data + 1);
714 point.z = *(input_->points[i_point].data + 2);
719 colored_cloud->
points.push_back (point);
722 std::vector< pcl::PointIndices >::iterator i_segment;
724 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
726 std::vector<int>::iterator i_point;
727 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
731 colored_cloud->
points[index].r = colors[3 * next_color];
732 colored_cloud->
points[index].g = colors[3 * next_color + 1];
733 colored_cloud->
points[index].b = colors[3 * next_color + 2];
739 return (colored_cloud);
742 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>;
744 #endif // PCL_SEGMENTATION_REGION_GROWING_HPP_