42 #include <pcl/segmentation/region_growing.h>
44 #include <pcl/point_cloud.h>
46 #include <pcl/common/point_tests.h>
47 #include <pcl/console/print.h>
48 #include <pcl/search/search.h>
49 #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<
pcl::
uindex_t>::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 ();
92 return (min_pts_per_cluster_);
96 template <
typename Po
intT,
typename NormalT>
void
99 min_pts_per_cluster_ = min_cluster_size;
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_ && !residual_flag_)
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_ && !residual_flag_)
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 auto cluster_iter_input = clusters.begin ();
279 for (
const auto& cluster : clusters_)
281 if ((cluster.indices.size () >= min_pts_per_cluster_) &&
282 (cluster.indices.size () <= max_pts_per_cluster_))
284 *cluster_iter_input = cluster;
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 ());
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 const auto 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 const auto 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);
387 for (
int i_point = 0; i_point < num_of_pts; i_point++)
389 const auto 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 const auto 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_)
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 for (
const auto& point : (*indices_))
586 point_was_found =
true;
592 if (clusters_.empty ())
594 point_neighbours_.clear ();
595 point_labels_.clear ();
596 num_pts_in_segment_.clear ();
597 number_of_segments_ = 0;
599 segmentation_is_possible = prepareForSegmentation ();
600 if ( !segmentation_is_possible )
606 findPointNeighbours ();
607 applySmoothRegionGrowingAlgorithm ();
612 for (
const auto& i_segment : clusters_)
614 const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
615 if (it != i_segment.indices.cend())
619 cluster.
indices.reserve (i_segment.indices.size ());
620 std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.
indices));
635 if (!clusters_.empty ())
639 srand (
static_cast<unsigned int> (time (
nullptr)));
640 std::vector<unsigned char> colors;
641 for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
643 colors.push_back (
static_cast<unsigned char> (rand () % 256));
644 colors.push_back (
static_cast<unsigned char> (rand () % 256));
645 colors.push_back (
static_cast<unsigned char> (rand () % 256));
648 colored_cloud->
width = input_->width;
649 colored_cloud->
height = input_->height;
650 colored_cloud->
is_dense = input_->is_dense;
651 for (
const auto& i_point: *input_)
654 point.x = *(i_point.data);
655 point.y = *(i_point.data + 1);
656 point.z = *(i_point.data + 2);
660 colored_cloud->
points.push_back (point);
664 for (
const auto& i_segment : clusters_)
666 for (
const auto& index : (i_segment.indices))
668 (*colored_cloud)[index].r = colors[3 * next_color];
669 (*colored_cloud)[index].g = colors[3 * next_color + 1];
670 (*colored_cloud)[index].b = colors[3 * next_color + 2];
676 return (colored_cloud);
685 if (!clusters_.empty ())
689 srand (
static_cast<unsigned int> (time (
nullptr)));
690 std::vector<unsigned char> colors;
691 for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
693 colors.push_back (
static_cast<unsigned char> (rand () % 256));
694 colors.push_back (
static_cast<unsigned char> (rand () % 256));
695 colors.push_back (
static_cast<unsigned char> (rand () % 256));
698 colored_cloud->
width = input_->width;
699 colored_cloud->
height = input_->height;
700 colored_cloud->
is_dense = input_->is_dense;
701 for (
const auto& i_point: *input_)
704 point.x = *(i_point.data);
705 point.y = *(i_point.data + 1);
706 point.z = *(i_point.data + 2);
711 colored_cloud->
points.push_back (point);
715 for (
const auto& i_segment : clusters_)
717 for (
const auto& index : (i_segment.indices))
719 (*colored_cloud)[index].r = colors[3 * next_color];
720 (*colored_cloud)[index].g = colors[3 * next_color + 1];
721 (*colored_cloud)[index].b = colors[3 * next_color + 2];
727 return (colored_cloud);
730 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>;
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
void setResidualThreshold(float residual)
Allows to set residual threshold used for testing the points.
~RegionGrowing() override
This destructor destroys the cloud, normals and search method used for finding KNN.
virtual void setCurvatureTestFlag(bool value)
Allows to turn on/off the curvature test.
typename KdTree::Ptr KdTreePtr
void setSearchMethod(const KdTreePtr &tree)
Allows to set search method that will be used for finding KNN.
void setInputNormals(const NormalPtr &norm)
This method sets the normals.
pcl::uindex_t getMinClusterSize()
Get the minimum number of points that a cluster needs to contain in order to be considered valid.
RegionGrowing()
Constructor that sets default values for member variables.
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.
virtual bool prepareForSegmentation()
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
virtual void findPointNeighbours()
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
NormalPtr getInputNormals() const
Returns normals.
float getCurvatureThreshold() const
Returns curvature threshold.
void setMinClusterSize(pcl::uindex_t min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid.
float getResidualThreshold() const
Returns residual threshold.
float getSmoothnessThreshold() const
Returns smoothness threshold.
KdTreePtr getSearchMethod() const
Returns the pointer to the search method that is used for KNN.
virtual void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
unsigned int getNumberOfNeighbours() const
Returns the number of nearest neighbours used for KNN.
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.
pcl::uindex_t getMaxClusterSize()
Get the maximum number of points that a cluster needs to contain in order to be considered valid.
virtual void getSegmentFromPoint(pcl::index_t index, pcl::PointIndices &cluster)
For a given point this function builds a segment to which it belongs and returns this segment.
virtual void setResidualTestFlag(bool value)
Allows to turn on/off the residual test.
void setMaxClusterSize(pcl::uindex_t max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid.
void setCurvatureThreshold(float curvature)
Allows to set curvature threshold used for testing the points.
void applySmoothRegionGrowingAlgorithm()
This function implements the algorithm described in the article "Segmentation of point clouds using s...
bool getSmoothModeFlag() const
Returns the flag value.
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud()
If the cloud was successfully segmented, then function returns colored cloud.
virtual bool validatePoint(pcl::index_t initial_seed, pcl::index_t point, pcl::index_t nghbr, bool &is_a_seed) const
This function is checking if the point with index 'nghbr' belongs to the segment.
void setSmoothModeFlag(bool value)
This function allows to turn on/off the smoothness constraint.
typename Normal::Ptr NormalPtr
void assembleRegions()
This function simply assembles the regions from list of point labels.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Defines all the PCL implemented PointT point type structures.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
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...
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing Euclidean xyz coordinates, and the RGB color.