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>
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 ((
static_cast<int> (cluster_iter->indices.size ()) >= min_pts_per_cluster_) &&
293 (
static_cast<int> (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);
361 if (input_->is_dense)
363 for (
int i_point = 0; i_point < point_number; i_point++)
365 int point_index = (*indices_)[i_point];
367 search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
368 point_neighbours_[point_index].swap (neighbours);
373 for (
int i_point = 0; i_point < point_number; i_point++)
376 int point_index = (*indices_)[i_point];
379 search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
380 point_neighbours_[point_index].swap (neighbours);
386 template <
typename Po
intT,
typename NormalT>
void
389 int num_of_pts =
static_cast<int> (indices_->size ());
390 point_labels_.resize (input_->points.size (), -1);
392 std::vector< std::pair<float, int> > point_residual;
393 std::pair<float, int> pair;
394 point_residual.resize (num_of_pts, pair);
396 if (normal_flag_ ==
true)
398 for (
int i_point = 0; i_point < num_of_pts; i_point++)
400 int point_index = (*indices_)[i_point];
401 point_residual[i_point].first = normals_->points[point_index].curvature;
402 point_residual[i_point].second = point_index;
404 std::sort (point_residual.begin (), point_residual.end (),
comparePair);
408 for (
int i_point = 0; i_point < num_of_pts; i_point++)
410 int point_index = (*indices_)[i_point];
411 point_residual[i_point].first = 0;
412 point_residual[i_point].second = point_index;
415 int seed_counter = 0;
416 int seed = point_residual[seed_counter].second;
418 int segmented_pts_num = 0;
419 int number_of_segments = 0;
420 while (segmented_pts_num < num_of_pts)
423 pts_in_segment = growRegion (seed, number_of_segments);
424 segmented_pts_num += pts_in_segment;
425 num_pts_in_segment_.push_back (pts_in_segment);
426 number_of_segments++;
429 for (
int i_seed = seed_counter + 1; i_seed < num_of_pts; i_seed++)
431 int index = point_residual[i_seed].second;
432 if (point_labels_[index] == -1)
435 seed_counter = i_seed;
443 template <
typename Po
intT,
typename NormalT>
int
446 std::queue<int> seeds;
447 seeds.push (initial_seed);
448 point_labels_[initial_seed] = segment_number;
450 int num_pts_in_segment = 1;
452 while (!seeds.empty ())
455 curr_seed = seeds.front ();
459 while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () )
461 int index = point_neighbours_[curr_seed][i_nghbr];
462 if (point_labels_[index] != -1)
468 bool is_a_seed =
false;
469 bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed);
471 if (belongs_to_segment ==
false)
477 point_labels_[index] = segment_number;
478 num_pts_in_segment++;
489 return (num_pts_in_segment);
493 template <
typename Po
intT,
typename NormalT>
bool
498 float cosine_threshold = cosf (theta_threshold_);
501 data[0] = input_->points[point].data[0];
502 data[1] = input_->points[point].data[1];
503 data[2] = input_->points[point].data[2];
504 data[3] = input_->points[point].data[3];
505 Eigen::Map<Eigen::Vector3f> initial_point (
static_cast<float*
> (data));
506 Eigen::Map<Eigen::Vector3f> initial_normal (
static_cast<float*
> (normals_->points[point].normal));
509 if (smooth_mode_flag_ ==
true)
511 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> (normals_->points[nghbr].normal));
512 float dot_product = fabsf (nghbr_normal.dot (initial_normal));
513 if (dot_product < cosine_threshold)
520 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> (normals_->points[nghbr].normal));
521 Eigen::Map<Eigen::Vector3f> initial_seed_normal (
static_cast<float*
> (normals_->points[initial_seed].normal));
522 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
523 if (dot_product < cosine_threshold)
528 if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
536 data_1[0] = input_->points[nghbr].data[0];
537 data_1[1] = input_->points[nghbr].data[1];
538 data_1[2] = input_->points[nghbr].data[2];
539 data_1[3] = input_->points[nghbr].data[3];
540 Eigen::Map<Eigen::Vector3f> nghbr_point (
static_cast<float*
> (data_1));
541 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
542 if (residual_flag_ && residual > residual_threshold_)
549 template <
typename Po
intT,
typename NormalT>
void
552 int number_of_segments =
static_cast<int> (num_pts_in_segment_.size ());
553 int number_of_points =
static_cast<int> (input_->points.size ());
556 clusters_.resize (number_of_segments, segment);
558 for (
int i_seg = 0; i_seg < number_of_segments; i_seg++)
560 clusters_[i_seg].
indices.resize ( num_pts_in_segment_[i_seg], 0);
563 std::vector<int> counter;
564 counter.resize (number_of_segments, 0);
566 for (
int i_point = 0; i_point < number_of_points; i_point++)
568 int segment_index = point_labels_[i_point];
569 if (segment_index != -1)
571 int point_index = counter[segment_index];
572 clusters_[segment_index].indices[point_index] = i_point;
573 counter[segment_index] = point_index + 1;
577 number_of_segments_ = number_of_segments;
581 template <
typename Po
intT,
typename NormalT>
void
586 bool segmentation_is_possible = initCompute ();
587 if ( !segmentation_is_possible )
594 bool point_was_found =
false;
595 int number_of_points =
static_cast <int> (indices_->size ());
596 for (
int point = 0; point < number_of_points; point++)
597 if ( (*indices_)[point] == index)
599 point_was_found =
true;
605 if (clusters_.empty ())
607 point_neighbours_.clear ();
608 point_labels_.clear ();
609 num_pts_in_segment_.clear ();
610 number_of_segments_ = 0;
612 segmentation_is_possible = prepareForSegmentation ();
613 if ( !segmentation_is_possible )
619 findPointNeighbours ();
620 applySmoothRegionGrowingAlgorithm ();
625 std::vector <pcl::PointIndices>::iterator i_segment;
626 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
628 bool segment_was_found =
false;
629 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
631 if (i_segment->indices[i_point] == index)
633 segment_was_found =
true;
635 cluster.
indices.reserve (i_segment->indices.size ());
636 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
640 if (segment_was_found)
656 if (!clusters_.empty ())
660 srand (
static_cast<unsigned int> (time (0)));
661 std::vector<unsigned char> colors;
662 for (
size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
664 colors.push_back (
static_cast<unsigned char> (rand () % 256));
665 colors.push_back (
static_cast<unsigned char> (rand () % 256));
666 colors.push_back (
static_cast<unsigned char> (rand () % 256));
669 colored_cloud->
width = input_->width;
670 colored_cloud->
height = input_->height;
671 colored_cloud->
is_dense = input_->is_dense;
672 for (
size_t i_point = 0; i_point < input_->points.size (); i_point++)
675 point.x = *(input_->points[i_point].data);
676 point.y = *(input_->points[i_point].data + 1);
677 point.z = *(input_->points[i_point].data + 2);
681 colored_cloud->
points.push_back (point);
684 std::vector< pcl::PointIndices >::iterator i_segment;
686 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
688 std::vector<int>::iterator i_point;
689 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
693 colored_cloud->
points[index].r = colors[3 * next_color];
694 colored_cloud->
points[index].g = colors[3 * next_color + 1];
695 colored_cloud->
points[index].b = colors[3 * next_color + 2];
701 return (colored_cloud);
710 if (!clusters_.empty ())
714 srand (
static_cast<unsigned int> (time (0)));
715 std::vector<unsigned char> colors;
716 for (
size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
718 colors.push_back (
static_cast<unsigned char> (rand () % 256));
719 colors.push_back (
static_cast<unsigned char> (rand () % 256));
720 colors.push_back (
static_cast<unsigned char> (rand () % 256));
723 colored_cloud->
width = input_->width;
724 colored_cloud->
height = input_->height;
725 colored_cloud->
is_dense = input_->is_dense;
726 for (
size_t i_point = 0; i_point < input_->points.size (); i_point++)
729 point.x = *(input_->points[i_point].data);
730 point.y = *(input_->points[i_point].data + 1);
731 point.z = *(input_->points[i_point].data + 2);
736 colored_cloud->
points.push_back (point);
739 std::vector< pcl::PointIndices >::iterator i_segment;
741 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
743 std::vector<int>::iterator i_point;
744 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
748 colored_cloud->
points[index].r = colors[3 * next_color];
749 colored_cloud->
points[index].g = colors[3 * next_color + 1];
750 colored_cloud->
points[index].b = colors[3 * next_color + 2];
756 return (colored_cloud);
759 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>;
761 #endif // PCL_SEGMENTATION_REGION_GROWING_HPP_