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_
float getCurvatureThreshold() const
Returns curvature threshold.
bool getCurvatureTestFlag() const
Returns the flag that signalize if the curvature test is turned on/off.
virtual ~RegionGrowing()
This destructor destroys the cloud, normals and search method used for finding KNN.
void setSmoothModeFlag(bool value)
This function allows to turn on/off the smoothness constraint.
bool getSmoothModeFlag() const
Returns the flag value.
virtual void setCurvatureTestFlag(bool value)
Allows to turn on/off the curvature test.
virtual bool prepareForSegmentation()
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
float getSmoothnessThreshold() const
Returns smoothness threshold.
void setInputNormals(const NormalPtr &norm)
This method sets the normals.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud()
If the cloud was successfully segmented, then function returns colored cloud.
virtual void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
float getResidualThreshold() const
Returns residual threshold.
uint32_t width
The point cloud width (if organized as an image-structure).
RegionGrowing()
Constructor that sets default values for member variables.
void setSearchMethod(const KdTreePtr &tree)
Allows to set search method that will be used for finding KNN.
bool getResidualTestFlag() const
Returns the flag that signalize if the residual test is turned on/off.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void setMinClusterSize(int min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid...
virtual void getSegmentFromPoint(int index, pcl::PointIndices &cluster)
For a given point this function builds a segment to which it belongs and returns this segment...
KdTreePtr getSearchMethod() const
Returns the pointer to the search method that is used for KNN.
void setSmoothnessThreshold(float theta)
Allows to set smoothness threshold used for testing the points.
virtual bool validatePoint(int initial_seed, int point, int nghbr, bool &is_a_seed) const
This function is checking if the point with index 'nghbr' belongs to the segment. ...
uint32_t height
The point cloud height (if organized as an image-structure).
A point structure representing Euclidean xyz coordinates, and the RGBA color.
int getMaxClusterSize()
Get the maximum number of points that a cluster needs to contain in order to be considered valid...
void setResidualThreshold(float residual)
Allows to set residual threshold used for testing the points.
NormalPtr getInputNormals() const
Returns normals.
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
std::vector< int > indices
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getColoredCloudRGBA()
If the cloud was successfully segmented, then function returns colored cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void setMaxClusterSize(int max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid...
void setNumberOfNeighbours(unsigned int neighbour_number)
Allows to set the number of neighbours.
boost::shared_ptr< PointCloud< PointT > > Ptr
void setCurvatureThreshold(float curvature)
Allows to set curvature threshold used for testing the points.
int growRegion(int initial_seed, int segment_number)
This method grows a segment for the given seed point.
unsigned int getNumberOfNeighbours() const
Returns the number of nearest neighbours used for KNN.
int getMinClusterSize()
Get the minimum number of points that a cluster needs to contain in order to be considered valid...
void assembleRegions()
This function simply assembles the regions from list of point labels.
virtual void findPointNeighbours()
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
void applySmoothRegionGrowingAlgorithm()
This function implements the algorithm described in the article "Segmentation of point clouds using s...
virtual void setResidualTestFlag(bool value)
Allows to turn on/off the residual test.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...