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
346 std::vector<float> distances;
348 point_neighbours_.resize (input_->size (), neighbours);
349 if (input_->is_dense)
351 for (
const auto& point_index: (*indices_))
354 search_->nearestKSearch (point_index, neighbour_number_, neighbours, distances);
355 point_neighbours_[point_index].swap (neighbours);
360 for (
const auto& point_index: (*indices_))
365 search_->nearestKSearch (point_index, neighbour_number_, neighbours, distances);
366 point_neighbours_[point_index].swap (neighbours);
372 template <
typename Po
intT,
typename NormalT>
void
375 int num_of_pts =
static_cast<int> (indices_->size ());
376 point_labels_.resize (input_->size (), -1);
378 std::vector< std::pair<float, int> > point_residual;
379 std::pair<float, int> pair;
380 point_residual.resize (num_of_pts, pair);
384 for (
int i_point = 0; i_point < num_of_pts; i_point++)
386 const auto point_index = (*indices_)[i_point];
387 point_residual[i_point].first = (*normals_)[point_index].curvature;
388 point_residual[i_point].second = point_index;
390 std::sort (point_residual.begin (), point_residual.end (),
comparePair);
394 for (
int i_point = 0; i_point < num_of_pts; i_point++)
396 const auto point_index = (*indices_)[i_point];
397 point_residual[i_point].first = 0;
398 point_residual[i_point].second = point_index;
401 int seed_counter = 0;
402 int seed = point_residual[seed_counter].second;
404 int segmented_pts_num = 0;
405 int number_of_segments = 0;
406 while (segmented_pts_num < num_of_pts)
409 pts_in_segment = growRegion (seed, number_of_segments);
410 segmented_pts_num += pts_in_segment;
411 num_pts_in_segment_.push_back (pts_in_segment);
412 number_of_segments++;
415 for (
int i_seed = seed_counter + 1; i_seed < num_of_pts; i_seed++)
417 int index = point_residual[i_seed].second;
418 if (point_labels_[index] == -1)
421 seed_counter = i_seed;
429 template <
typename Po
intT,
typename NormalT>
int
432 std::queue<int> seeds;
433 seeds.push (initial_seed);
434 point_labels_[initial_seed] = segment_number;
436 int num_pts_in_segment = 1;
438 while (!seeds.empty ())
441 curr_seed = seeds.front ();
444 std::size_t i_nghbr = 0;
445 while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () )
447 int index = point_neighbours_[curr_seed][i_nghbr];
448 if (point_labels_[index] != -1)
454 bool is_a_seed =
false;
455 bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed);
457 if (!belongs_to_segment)
463 point_labels_[index] = segment_number;
464 num_pts_in_segment++;
475 return (num_pts_in_segment);
479 template <
typename Po
intT,
typename NormalT>
bool
484 float cosine_threshold = std::cos (theta_threshold_);
487 data[0] = (*input_)[point].data[0];
488 data[1] = (*input_)[point].data[1];
489 data[2] = (*input_)[point].data[2];
490 data[3] = (*input_)[point].data[3];
491 Eigen::Map<Eigen::Vector3f> initial_point (
static_cast<float*
> (data));
492 Eigen::Map<Eigen::Vector3f> initial_normal (
static_cast<float*
> ((*normals_)[point].normal));
495 if (smooth_mode_flag_)
497 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
498 float dot_product = std::abs (nghbr_normal.dot (initial_normal));
499 if (dot_product < cosine_threshold)
506 Eigen::Map<Eigen::Vector3f> nghbr_normal (
static_cast<float*
> ((*normals_)[nghbr].normal));
507 Eigen::Map<Eigen::Vector3f> initial_seed_normal (
static_cast<float*
> ((*normals_)[initial_seed].normal));
508 float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
509 if (dot_product < cosine_threshold)
514 if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
522 data_1[0] = (*input_)[nghbr].data[0];
523 data_1[1] = (*input_)[nghbr].data[1];
524 data_1[2] = (*input_)[nghbr].data[2];
525 data_1[3] = (*input_)[nghbr].data[3];
526 Eigen::Map<Eigen::Vector3f> nghbr_point (
static_cast<float*
> (data_1));
527 float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
528 if (residual_flag_ && residual > residual_threshold_)
535 template <
typename Po
intT,
typename NormalT>
void
538 const auto number_of_segments = num_pts_in_segment_.size ();
539 const auto number_of_points = input_->size ();
542 clusters_.resize (number_of_segments, segment);
544 for (std::size_t i_seg = 0; i_seg < number_of_segments; i_seg++)
546 clusters_[i_seg].
indices.resize ( num_pts_in_segment_[i_seg], 0);
549 std::vector<int> counter(number_of_segments, 0);
551 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
553 const auto segment_index = point_labels_[i_point];
554 if (segment_index != -1)
556 const auto point_index = counter[segment_index];
557 clusters_[segment_index].indices[point_index] = i_point;
558 counter[segment_index] = point_index + 1;
562 number_of_segments_ = number_of_segments;
566 template <
typename Po
intT,
typename NormalT>
void
571 bool segmentation_is_possible = initCompute ();
572 if ( !segmentation_is_possible )
579 bool point_was_found =
false;
580 for (
const auto& point : (*indices_))
583 point_was_found =
true;
589 if (clusters_.empty ())
591 point_neighbours_.clear ();
592 point_labels_.clear ();
593 num_pts_in_segment_.clear ();
594 number_of_segments_ = 0;
596 segmentation_is_possible = prepareForSegmentation ();
597 if ( !segmentation_is_possible )
603 findPointNeighbours ();
604 applySmoothRegionGrowingAlgorithm ();
609 for (
const auto& i_segment : clusters_)
611 const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
612 if (it != i_segment.indices.cend())
616 cluster.
indices.reserve (i_segment.indices.size ());
617 std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.
indices));
632 if (!clusters_.empty ())
636 srand (
static_cast<unsigned int> (time (
nullptr)));
637 std::vector<unsigned char> colors;
638 for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
640 colors.push_back (
static_cast<unsigned char> (rand () % 256));
641 colors.push_back (
static_cast<unsigned char> (rand () % 256));
642 colors.push_back (
static_cast<unsigned char> (rand () % 256));
645 colored_cloud->
width = input_->width;
646 colored_cloud->
height = input_->height;
647 colored_cloud->
is_dense = input_->is_dense;
648 for (
const auto& i_point: *input_)
651 point.x = *(i_point.data);
652 point.y = *(i_point.data + 1);
653 point.z = *(i_point.data + 2);
657 colored_cloud->
points.push_back (point);
661 for (
const auto& i_segment : clusters_)
663 for (
const auto& index : (i_segment.indices))
665 (*colored_cloud)[index].r = colors[3 * next_color];
666 (*colored_cloud)[index].g = colors[3 * next_color + 1];
667 (*colored_cloud)[index].b = colors[3 * next_color + 2];
673 return (colored_cloud);
682 if (!clusters_.empty ())
686 srand (
static_cast<unsigned int> (time (
nullptr)));
687 std::vector<unsigned char> colors;
688 for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
690 colors.push_back (
static_cast<unsigned char> (rand () % 256));
691 colors.push_back (
static_cast<unsigned char> (rand () % 256));
692 colors.push_back (
static_cast<unsigned char> (rand () % 256));
695 colored_cloud->
width = input_->width;
696 colored_cloud->
height = input_->height;
697 colored_cloud->
is_dense = input_->is_dense;
698 for (
const auto& i_point: *input_)
701 point.x = *(i_point.data);
702 point.y = *(i_point.data + 1);
703 point.z = *(i_point.data + 2);
708 colored_cloud->
points.push_back (point);
712 for (
const auto& i_segment : clusters_)
714 for (
const auto& index : (i_segment.indices))
716 (*colored_cloud)[index].r = colors[3 * next_color];
717 (*colored_cloud)[index].g = colors[3 * next_color + 1];
718 (*colored_cloud)[index].b = colors[3 * next_color + 2];
724 return (colored_cloud);
727 #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.