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);
306 template <
typename Po
intT,
typename NormalT>
bool
310 if ( input_->points.size () == 0 )
314 if ( normals_ == 0 || input_->points.size () != normals_->points.size () )
320 if (residual_threshold_ <= 0.0f)
332 if (neighbour_number_ == 0)
341 if (indices_->empty ())
342 PCL_ERROR (
"[pcl::RegionGrowing::prepareForSegmentation] Empty given indices!\n");
343 search_->setInputCloud (input_, indices_);
346 search_->setInputCloud (input_);
352 template <
typename Po
intT,
typename NormalT>
void
355 int point_number =
static_cast<int> (indices_->size ());
356 std::vector<int> neighbours;
357 std::vector<float> distances;
359 point_neighbours_.resize (input_->points.size (), neighbours);
361 for (
int i_point = 0; i_point < point_number; i_point++)
363 int point_index = (*indices_)[i_point];
365 search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
366 point_neighbours_[point_index].swap (neighbours);
371 template <
typename Po
intT,
typename NormalT>
void
374 int num_of_pts =
static_cast<int> (indices_->size ());
375 point_labels_.resize (input_->points.size (), -1);
377 std::vector< std::pair<float, int> > point_residual;
378 std::pair<float, int> pair;
379 point_residual.resize (num_of_pts, pair);
381 if (normal_flag_ ==
true)
383 for (
int i_point = 0; i_point < num_of_pts; i_point++)
385 int point_index = (*indices_)[i_point];
386 point_residual[i_point].first = normals_->points[point_index].curvature;
387 point_residual[i_point].second = point_index;
389 std::sort (point_residual.begin (), point_residual.end (),
comparePair);
393 for (
int i_point = 0; i_point < num_of_pts; i_point++)
395 int point_index = (*indices_)[i_point];
396 point_residual[i_point].first = 0;
397 point_residual[i_point].second = point_index;
400 int seed_counter = 0;
401 int seed = point_residual[seed_counter].second;
403 int segmented_pts_num = 0;
404 int number_of_segments = 0;
405 while (segmented_pts_num < num_of_pts)
408 pts_in_segment = growRegion (seed, number_of_segments);
409 segmented_pts_num += pts_in_segment;
410 num_pts_in_segment_.push_back (pts_in_segment);
411 number_of_segments++;
414 for (
int i_seed = seed_counter + 1; i_seed < num_of_pts; i_seed++)
416 int index = point_residual[i_seed].second;
417 if (point_labels_[index] == -1)
427 template <
typename Po
intT,
typename NormalT>
int
430 std::queue<int> seeds;
431 seeds.push (initial_seed);
432 point_labels_[initial_seed] = segment_number;
434 int num_pts_in_segment = 1;
436 while (!seeds.empty ())
439 curr_seed = seeds.front ();
443 while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () )
445 int index = point_neighbours_[curr_seed][i_nghbr];
446 if (point_labels_[index] != -1)
452 bool is_a_seed =
false;
453 bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed);
455 if (belongs_to_segment ==
false)
461 point_labels_[index] = segment_number;
462 num_pts_in_segment++;
473 return (num_pts_in_segment);
477 template <
typename Po
intT,
typename NormalT>
bool
482 float cosine_threshold = cosf (theta_threshold_);
485 data[0] = input_->points[point].data[0];
486 data[1] = input_->points[point].data[1];
487 data[2] = input_->points[point].data[2];
488 data[3] = input_->points[point].data[3];
489 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
490 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
493 if (smooth_mode_flag_ ==
true)
495 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
496 float dot_product = fabsf (nghbr_normal.dot (initial_normal));
497 if (dot_product < cosine_threshold)
504 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
505 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal));
506 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
507 if (dot_product < cosine_threshold)
512 if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
518 data[0] = input_->points[nghbr].data[0];
519 data[1] = input_->points[nghbr].data[1];
520 data[2] = input_->points[nghbr].data[2];
521 data[3] = input_->points[nghbr].data[3];
522 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data));
523 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
524 if (residual_flag_ && residual > residual_threshold_)
531 template <
typename Po
intT,
typename NormalT>
void
534 int number_of_segments =
static_cast<int> (num_pts_in_segment_.size ());
535 int number_of_points =
static_cast<int> (input_->points.size ());
538 clusters_.resize (number_of_segments, segment);
540 for (
int i_seg = 0; i_seg < number_of_segments; i_seg++)
542 clusters_[i_seg].
indices.resize ( num_pts_in_segment_[i_seg], 0);
545 std::vector<int> counter;
546 counter.resize (number_of_segments, 0);
548 for (
int i_point = 0; i_point < number_of_points; i_point++)
550 int segment_index = point_labels_[i_point];
551 if (segment_index != -1)
553 int point_index = counter[segment_index];
554 clusters_[segment_index].indices[point_index] = i_point;
555 counter[segment_index] = point_index + 1;
559 number_of_segments_ = number_of_segments;
563 template <
typename Po
intT,
typename NormalT>
void
568 bool segmentation_is_possible = initCompute ();
569 if ( !segmentation_is_possible )
576 bool point_was_found =
false;
577 int number_of_points = static_cast <
int> (indices_->size ());
578 for (
size_t point = 0; point < number_of_points; point++)
579 if ( (*indices_)[point] == index)
581 point_was_found =
true;
587 if (clusters_.empty ())
589 point_neighbours_.clear ();
590 point_labels_.clear ();
591 num_pts_in_segment_.clear ();
592 number_of_segments_ = 0;
594 segmentation_is_possible = prepareForSegmentation ();
595 if ( !segmentation_is_possible )
601 findPointNeighbours ();
602 applySmoothRegionGrowingAlgorithm ();
607 std::vector <pcl::PointIndices>::iterator i_segment;
608 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
610 bool segment_was_found =
false;
611 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
613 if (i_segment->indices[i_point] == index)
615 segment_was_found =
true;
617 cluster.
indices.reserve (i_segment->indices.size ());
618 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
622 if (segment_was_found)
638 if (!clusters_.empty ())
642 srand (static_cast<unsigned int> (time (0)));
643 std::vector<unsigned char> colors;
644 for (
size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
646 colors.push_back (static_cast<unsigned char> (rand () % 256));
647 colors.push_back (static_cast<unsigned char> (rand () % 256));
648 colors.push_back (static_cast<unsigned char> (rand () % 256));
651 colored_cloud->
width = input_->width;
652 colored_cloud->
height = input_->height;
653 colored_cloud->
is_dense = input_->is_dense;
654 for (
size_t i_point = 0; i_point < input_->points.size (); i_point++)
657 point.x = *(input_->points[i_point].data);
658 point.y = *(input_->points[i_point].data + 1);
659 point.z = *(input_->points[i_point].data + 2);
663 colored_cloud->
points.push_back (point);
666 std::vector< pcl::PointIndices >::iterator i_segment;
668 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
670 std::vector<int>::iterator i_point;
671 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
675 colored_cloud->
points[index].r = colors[3 * next_color];
676 colored_cloud->
points[index].g = colors[3 * next_color + 1];
677 colored_cloud->
points[index].b = colors[3 * next_color + 2];
683 return (colored_cloud);
692 if (!clusters_.empty ())
696 srand (static_cast<unsigned int> (time (0)));
697 std::vector<unsigned char> colors;
698 for (
size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
700 colors.push_back (static_cast<unsigned char> (rand () % 256));
701 colors.push_back (static_cast<unsigned char> (rand () % 256));
702 colors.push_back (static_cast<unsigned char> (rand () % 256));
705 colored_cloud->
width = input_->width;
706 colored_cloud->
height = input_->height;
707 colored_cloud->
is_dense = input_->is_dense;
708 for (
size_t i_point = 0; i_point < input_->points.size (); i_point++)
711 point.x = *(input_->points[i_point].data);
712 point.y = *(input_->points[i_point].data + 1);
713 point.z = *(input_->points[i_point].data + 2);
718 colored_cloud->
points.push_back (point);
721 std::vector< pcl::PointIndices >::iterator i_segment;
723 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
725 std::vector<int>::iterator i_point;
726 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
730 colored_cloud->
points[index].r = colors[3 * next_color];
731 colored_cloud->
points[index].g = colors[3 * next_color + 1];
732 colored_cloud->
points[index].b = colors[3 * next_color + 2];
738 return (colored_cloud);
741 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>;
743 #endif // PCL_SEGMENTATION_REGION_GROWING_HPP_