42 #include <pcl/segmentation/region_growing.h> 44 #include <pcl/point_cloud.h> 46 #include <pcl/common/point_tests.h> 47 #include <pcl/search/search.h> 48 #include <pcl/search/kdtree.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>
82 point_neighbours_.clear ();
83 point_labels_.clear ();
84 num_pts_in_segment_.clear ();
89 template <
typename Po
intT,
typename NormalT>
int 92 return (min_pts_per_cluster_);
96 template <
typename Po
intT,
typename NormalT>
void 99 min_pts_per_cluster_ = min_cluster_size;
103 template <
typename Po
intT,
typename NormalT>
int 106 return (max_pts_per_cluster_);
110 template <
typename Po
intT,
typename NormalT>
void 113 max_pts_per_cluster_ = max_cluster_size;
117 template <
typename Po
intT,
typename NormalT>
bool 120 return (smooth_mode_flag_);
124 template <
typename Po
intT,
typename NormalT>
void 127 smooth_mode_flag_ = value;
131 template <
typename Po
intT,
typename NormalT>
bool 134 return (curvature_flag_);
138 template <
typename Po
intT,
typename NormalT>
void 141 curvature_flag_ = value;
143 if (curvature_flag_ ==
false && residual_flag_ ==
false)
144 residual_flag_ =
true;
148 template <
typename Po
intT,
typename NormalT>
bool 151 return (residual_flag_);
155 template <
typename Po
intT,
typename NormalT>
void 158 residual_flag_ = value;
160 if (curvature_flag_ ==
false && residual_flag_ ==
false)
161 curvature_flag_ =
true;
165 template <
typename Po
intT,
typename NormalT>
float 168 return (theta_threshold_);
172 template <
typename Po
intT,
typename NormalT>
void 175 theta_threshold_ = theta;
179 template <
typename Po
intT,
typename NormalT>
float 182 return (residual_threshold_);
186 template <
typename Po
intT,
typename NormalT>
void 189 residual_threshold_ = residual;
193 template <
typename Po
intT,
typename NormalT>
float 196 return (curvature_threshold_);
200 template <
typename Po
intT,
typename NormalT>
void 203 curvature_threshold_ = curvature;
207 template <
typename Po
intT,
typename NormalT>
unsigned int 210 return (neighbour_number_);
214 template <
typename Po
intT,
typename NormalT>
void 217 neighbour_number_ = neighbour_number;
228 template <
typename Po
intT,
typename NormalT>
void 242 template <
typename Po
intT,
typename NormalT>
void 249 template <
typename Po
intT,
typename NormalT>
void 254 point_neighbours_.clear ();
255 point_labels_.clear ();
256 num_pts_in_segment_.clear ();
257 number_of_segments_ = 0;
259 bool segmentation_is_possible = initCompute ();
260 if ( !segmentation_is_possible )
266 segmentation_is_possible = prepareForSegmentation ();
267 if ( !segmentation_is_possible )
273 findPointNeighbours ();
274 applySmoothRegionGrowingAlgorithm ();
277 clusters.resize (clusters_.size ());
278 std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin ();
279 for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); ++cluster_iter)
281 if ((static_cast<int> (cluster_iter->indices.size ()) >= min_pts_per_cluster_) &&
282 (
static_cast<int> (cluster_iter->indices.size ()) <= max_pts_per_cluster_))
284 *cluster_iter_input = *cluster_iter;
285 ++cluster_iter_input;
289 clusters_ = std::vector<pcl::PointIndices> (clusters.begin (), cluster_iter_input);
290 clusters.resize(clusters_.size());
296 template <
typename Po
intT,
typename NormalT>
bool 300 if ( input_->points.empty () )
304 if ( !normals_ || input_->size () != normals_->size () )
310 if (residual_threshold_ <= 0.0f)
322 if (neighbour_number_ == 0)
331 if (indices_->empty ())
332 PCL_ERROR (
"[pcl::RegionGrowing::prepareForSegmentation] Empty given indices!\n");
333 search_->setInputCloud (input_, indices_);
336 search_->setInputCloud (input_);
342 template <
typename Po
intT,
typename NormalT>
void 345 int point_number =
static_cast<int> (indices_->size ());
346 std::vector<int> neighbours;
347 std::vector<float> distances;
349 point_neighbours_.resize (input_->size (), neighbours);
350 if (input_->is_dense)
352 for (
int i_point = 0; i_point < point_number; i_point++)
354 int point_index = (*indices_)[i_point];
356 search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
357 point_neighbours_[point_index].swap (neighbours);
362 for (
int i_point = 0; i_point < point_number; i_point++)
365 int point_index = (*indices_)[i_point];
368 search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
369 point_neighbours_[point_index].swap (neighbours);
375 template <
typename Po
intT,
typename NormalT>
void 378 int num_of_pts =
static_cast<int> (indices_->size ());
379 point_labels_.resize (input_->size (), -1);
381 std::vector< std::pair<float, int> > point_residual;
382 std::pair<float, int> pair;
383 point_residual.resize (num_of_pts, pair);
385 if (normal_flag_ ==
true)
387 for (
int i_point = 0; i_point < num_of_pts; i_point++)
389 int point_index = (*indices_)[i_point];
390 point_residual[i_point].first = (*normals_)[point_index].curvature;
391 point_residual[i_point].second = point_index;
393 std::sort (point_residual.begin (), point_residual.end (),
comparePair);
397 for (
int i_point = 0; i_point < num_of_pts; i_point++)
399 int point_index = (*indices_)[i_point];
400 point_residual[i_point].first = 0;
401 point_residual[i_point].second = point_index;
404 int seed_counter = 0;
405 int seed = point_residual[seed_counter].second;
407 int segmented_pts_num = 0;
408 int number_of_segments = 0;
409 while (segmented_pts_num < num_of_pts)
412 pts_in_segment = growRegion (seed, number_of_segments);
413 segmented_pts_num += pts_in_segment;
414 num_pts_in_segment_.push_back (pts_in_segment);
415 number_of_segments++;
418 for (
int i_seed = seed_counter + 1; i_seed < num_of_pts; i_seed++)
420 int index = point_residual[i_seed].second;
421 if (point_labels_[index] == -1)
424 seed_counter = i_seed;
432 template <
typename Po
intT,
typename NormalT>
int 435 std::queue<int> seeds;
436 seeds.push (initial_seed);
437 point_labels_[initial_seed] = segment_number;
439 int num_pts_in_segment = 1;
441 while (!seeds.empty ())
444 curr_seed = seeds.front ();
447 std::size_t i_nghbr = 0;
448 while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () )
450 int index = point_neighbours_[curr_seed][i_nghbr];
451 if (point_labels_[index] != -1)
457 bool is_a_seed =
false;
458 bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed);
460 if (!belongs_to_segment)
466 point_labels_[index] = segment_number;
467 num_pts_in_segment++;
478 return (num_pts_in_segment);
482 template <
typename Po
intT,
typename NormalT>
bool 487 float cosine_threshold = std::cos (theta_threshold_);
490 data[0] = (*input_)[point].data[0];
491 data[1] = (*input_)[point].data[1];
492 data[2] = (*input_)[point].data[2];
493 data[3] = (*input_)[point].data[3];
494 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
495 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
498 if (smooth_mode_flag_ ==
true)
500 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
501 float dot_product = std::abs (nghbr_normal.dot (initial_normal));
502 if (dot_product < cosine_threshold)
509 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
510 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> ((*normals_)[initial_seed].normal));
511 float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
512 if (dot_product < cosine_threshold)
517 if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
525 data_1[0] = (*input_)[nghbr].data[0];
526 data_1[1] = (*input_)[nghbr].data[1];
527 data_1[2] = (*input_)[nghbr].data[2];
528 data_1[3] = (*input_)[nghbr].data[3];
529 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_1));
530 float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
531 if (residual_flag_ && residual > residual_threshold_)
538 template <
typename Po
intT,
typename NormalT>
void 541 const auto number_of_segments = num_pts_in_segment_.size ();
542 const auto number_of_points = input_->size ();
545 clusters_.resize (number_of_segments, segment);
547 for (std::size_t i_seg = 0; i_seg < number_of_segments; i_seg++)
549 clusters_[i_seg].
indices.resize ( num_pts_in_segment_[i_seg], 0);
552 std::vector<int> counter(number_of_segments, 0);
554 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
556 const auto segment_index = point_labels_[i_point];
557 if (segment_index != -1)
559 const auto point_index = counter[segment_index];
560 clusters_[segment_index].indices[point_index] = i_point;
561 counter[segment_index] = point_index + 1;
565 number_of_segments_ = number_of_segments;
569 template <
typename Po
intT,
typename NormalT>
void 574 bool segmentation_is_possible = initCompute ();
575 if ( !segmentation_is_possible )
582 bool point_was_found =
false;
583 int number_of_points = static_cast <
int> (indices_->size ());
584 for (
int point = 0; point < number_of_points; point++)
585 if ( (*indices_)[point] == index)
587 point_was_found =
true;
593 if (clusters_.empty ())
595 point_neighbours_.clear ();
596 point_labels_.clear ();
597 num_pts_in_segment_.clear ();
598 number_of_segments_ = 0;
600 segmentation_is_possible = prepareForSegmentation ();
601 if ( !segmentation_is_possible )
607 findPointNeighbours ();
608 applySmoothRegionGrowingAlgorithm ();
613 for (
auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
615 bool segment_was_found =
false;
616 for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
618 if (i_segment->indices[i_point] == index)
620 segment_was_found =
true;
622 cluster.
indices.reserve (i_segment->indices.size ());
623 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
627 if (segment_was_found)
643 if (!clusters_.empty ())
647 srand (static_cast<unsigned int> (time (
nullptr)));
648 std::vector<unsigned char> colors;
649 for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
651 colors.push_back (static_cast<unsigned char> (rand () % 256));
652 colors.push_back (static_cast<unsigned char> (rand () % 256));
653 colors.push_back (static_cast<unsigned char> (rand () % 256));
656 colored_cloud->
width = input_->width;
657 colored_cloud->
height = input_->height;
658 colored_cloud->
is_dense = input_->is_dense;
659 for (
const auto& i_point: *input_)
662 point.x = *(i_point.data);
663 point.y = *(i_point.data + 1);
664 point.z = *(i_point.data + 2);
668 colored_cloud->
points.push_back (point);
672 for (
auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
674 for (
auto i_point = i_segment->indices.cbegin (); i_point != i_segment->indices.cend (); i_point++)
678 (*colored_cloud)[index].r = colors[3 * next_color];
679 (*colored_cloud)[index].g = colors[3 * next_color + 1];
680 (*colored_cloud)[index].b = colors[3 * next_color + 2];
686 return (colored_cloud);
695 if (!clusters_.empty ())
699 srand (static_cast<unsigned int> (time (
nullptr)));
700 std::vector<unsigned char> colors;
701 for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
703 colors.push_back (static_cast<unsigned char> (rand () % 256));
704 colors.push_back (static_cast<unsigned char> (rand () % 256));
705 colors.push_back (static_cast<unsigned char> (rand () % 256));
708 colored_cloud->
width = input_->width;
709 colored_cloud->
height = input_->height;
710 colored_cloud->
is_dense = input_->is_dense;
711 for (
const auto& i_point: *input_)
714 point.x = *(i_point.data);
715 point.y = *(i_point.data + 1);
716 point.z = *(i_point.data + 2);
721 colored_cloud->
points.push_back (point);
725 for (
auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
727 for (
auto i_point = i_segment->indices.cbegin (); i_point != i_segment->indices.cend (); i_point++)
729 int index = *i_point;
730 (*colored_cloud)[index].r = colors[3 * next_color];
731 (*colored_cloud)[index].g = colors[3 * next_color + 1];
732 (*colored_cloud)[index].b = colors[3 * next_color + 2];
738 return (colored_cloud);
741 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>; unsigned int getNumberOfNeighbours() const
Returns the number of nearest neighbours used for KNN.
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...
float getResidualThreshold() const
Returns residual threshold.
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
KdTreePtr getSearchMethod() const
Returns the pointer to the search method that is used for KNN.
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...
~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.
typename KdTree::Ptr KdTreePtr
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.
void setSmoothnessThreshold(float theta)
Allows to set smoothness threshold used for testing the points.
float getSmoothnessThreshold() const
Returns smoothness threshold.
std::uint32_t width
The point cloud width (if organized as an image-structure).
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.
Defines all the PCL implemented PointT point type structures.
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.
NormalPtr getInputNormals() const
Returns normals.
float getCurvatureThreshold() const
Returns curvature threshold.
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 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...
std::uint32_t height
The point cloud height (if organized as an image-structure).
bool getResidualTestFlag() const
Returns the flag that signalize if the residual test is turned on/off.
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getColoredCloudRGBA()
If the cloud was successfully segmented, then function returns colored cloud.
bool getSmoothModeFlag() const
Returns the flag value.
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...
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
virtual void setCurvatureTestFlag(bool value)
Allows to turn on/off the curvature test.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool getCurvatureTestFlag() const
Returns the flag that signalize if the curvature test is turned on/off.
void setSearchMethod(const KdTreePtr &tree)
Allows to set search method that will be used for finding KNN.
typename Normal::Ptr NormalPtr