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)
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_
boost::shared_ptr< PointCloud< PointT > > Ptr
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 return true if f...
uint32_t width
The point cloud width (if organized as an image-structure).
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...
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.
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...
uint32_t height
The point cloud height (if organized as an image-structure).
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 in any of their floating point fields)...