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 ((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)
442 template <
typename Po
intT,
typename NormalT>
int
445 std::queue<int> seeds;
446 seeds.push (initial_seed);
447 point_labels_[initial_seed] = segment_number;
449 int num_pts_in_segment = 1;
451 while (!seeds.empty ())
454 curr_seed = seeds.front ();
458 while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () )
460 int index = point_neighbours_[curr_seed][i_nghbr];
461 if (point_labels_[index] != -1)
467 bool is_a_seed =
false;
468 bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed);
470 if (belongs_to_segment ==
false)
476 point_labels_[index] = segment_number;
477 num_pts_in_segment++;
488 return (num_pts_in_segment);
492 template <
typename Po
intT,
typename NormalT>
bool
497 float cosine_threshold = cosf (theta_threshold_);
500 data[0] = input_->points[point].data[0];
501 data[1] = input_->points[point].data[1];
502 data[2] = input_->points[point].data[2];
503 data[3] = input_->points[point].data[3];
504 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
505 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
508 if (smooth_mode_flag_ ==
true)
510 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
511 float dot_product = fabsf (nghbr_normal.dot (initial_normal));
512 if (dot_product < cosine_threshold)
519 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
520 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal));
521 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
522 if (dot_product < cosine_threshold)
527 if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
533 data[0] = input_->points[nghbr].data[0];
534 data[1] = input_->points[nghbr].data[1];
535 data[2] = input_->points[nghbr].data[2];
536 data[3] = input_->points[nghbr].data[3];
537 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data));
538 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
539 if (residual_flag_ && residual > residual_threshold_)
546 template <
typename Po
intT,
typename NormalT>
void
549 int number_of_segments =
static_cast<int> (num_pts_in_segment_.size ());
550 int number_of_points =
static_cast<int> (input_->points.size ());
553 clusters_.resize (number_of_segments, segment);
555 for (
int i_seg = 0; i_seg < number_of_segments; i_seg++)
557 clusters_[i_seg].
indices.resize ( num_pts_in_segment_[i_seg], 0);
560 std::vector<int> counter;
561 counter.resize (number_of_segments, 0);
563 for (
int i_point = 0; i_point < number_of_points; i_point++)
565 int segment_index = point_labels_[i_point];
566 if (segment_index != -1)
568 int point_index = counter[segment_index];
569 clusters_[segment_index].indices[point_index] = i_point;
570 counter[segment_index] = point_index + 1;
574 number_of_segments_ = number_of_segments;
578 template <
typename Po
intT,
typename NormalT>
void
583 bool segmentation_is_possible = initCompute ();
584 if ( !segmentation_is_possible )
591 bool point_was_found =
false;
592 int number_of_points = static_cast <
int> (indices_->size ());
593 for (
int point = 0; point < number_of_points; point++)
594 if ( (*indices_)[point] == index)
596 point_was_found =
true;
602 if (clusters_.empty ())
604 point_neighbours_.clear ();
605 point_labels_.clear ();
606 num_pts_in_segment_.clear ();
607 number_of_segments_ = 0;
609 segmentation_is_possible = prepareForSegmentation ();
610 if ( !segmentation_is_possible )
616 findPointNeighbours ();
617 applySmoothRegionGrowingAlgorithm ();
622 std::vector <pcl::PointIndices>::iterator i_segment;
623 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
625 bool segment_was_found =
false;
626 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
628 if (i_segment->indices[i_point] == index)
630 segment_was_found =
true;
632 cluster.
indices.reserve (i_segment->indices.size ());
633 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
637 if (segment_was_found)
653 if (!clusters_.empty ())
657 srand (static_cast<unsigned int> (time (0)));
658 std::vector<unsigned char> colors;
659 for (
size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
661 colors.push_back (static_cast<unsigned char> (rand () % 256));
662 colors.push_back (static_cast<unsigned char> (rand () % 256));
663 colors.push_back (static_cast<unsigned char> (rand () % 256));
666 colored_cloud->
width = input_->width;
667 colored_cloud->
height = input_->height;
668 colored_cloud->
is_dense = input_->is_dense;
669 for (
size_t i_point = 0; i_point < input_->points.size (); i_point++)
672 point.x = *(input_->points[i_point].data);
673 point.y = *(input_->points[i_point].data + 1);
674 point.z = *(input_->points[i_point].data + 2);
678 colored_cloud->
points.push_back (point);
681 std::vector< pcl::PointIndices >::iterator i_segment;
683 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
685 std::vector<int>::iterator i_point;
686 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
690 colored_cloud->
points[index].r = colors[3 * next_color];
691 colored_cloud->
points[index].g = colors[3 * next_color + 1];
692 colored_cloud->
points[index].b = colors[3 * next_color + 2];
698 return (colored_cloud);
707 if (!clusters_.empty ())
711 srand (static_cast<unsigned int> (time (0)));
712 std::vector<unsigned char> colors;
713 for (
size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
715 colors.push_back (static_cast<unsigned char> (rand () % 256));
716 colors.push_back (static_cast<unsigned char> (rand () % 256));
717 colors.push_back (static_cast<unsigned char> (rand () % 256));
720 colored_cloud->
width = input_->width;
721 colored_cloud->
height = input_->height;
722 colored_cloud->
is_dense = input_->is_dense;
723 for (
size_t i_point = 0; i_point < input_->points.size (); i_point++)
726 point.x = *(input_->points[i_point].data);
727 point.y = *(input_->points[i_point].data + 1);
728 point.z = *(input_->points[i_point].data + 2);
733 colored_cloud->
points.push_back (point);
736 std::vector< pcl::PointIndices >::iterator i_segment;
738 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
740 std::vector<int>::iterator i_point;
741 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
745 colored_cloud->
points[index].r = colors[3 * next_color];
746 colored_cloud->
points[index].g = colors[3 * next_color + 1];
747 colored_cloud->
points[index].b = colors[3 * next_color + 2];
753 return (colored_cloud);
756 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>;
758 #endif // PCL_SEGMENTATION_REGION_GROWING_HPP_
float getResidualThreshold() const
Returns residual threshold.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
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).