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