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>
56template <
typename Po
intT,
typename NormalT>
58 min_pts_per_cluster_ (1),
60 smooth_mode_flag_ (
true),
61 curvature_flag_ (
true),
62 residual_flag_ (
false),
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)
79template <
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_);
96template <
typename Po
intT,
typename NormalT>
void
99 min_pts_per_cluster_ = min_cluster_size;
106 return (max_pts_per_cluster_);
110template <
typename Po
intT,
typename NormalT>
void
113 max_pts_per_cluster_ = max_cluster_size;
117template <
typename Po
intT,
typename NormalT>
bool
120 return (smooth_mode_flag_);
124template <
typename Po
intT,
typename NormalT>
void
127 smooth_mode_flag_ = value;
131template <
typename Po
intT,
typename NormalT>
bool
134 return (curvature_flag_);
138template <
typename Po
intT,
typename NormalT>
void
141 curvature_flag_ = value;
143 if (curvature_flag_ ==
false && residual_flag_ ==
false)
144 residual_flag_ =
true;
148template <
typename Po
intT,
typename NormalT>
bool
151 return (residual_flag_);
155template <
typename Po
intT,
typename NormalT>
void
158 residual_flag_ = value;
160 if (curvature_flag_ ==
false && residual_flag_ ==
false)
161 curvature_flag_ =
true;
165template <
typename Po
intT,
typename NormalT>
float
168 return (theta_threshold_);
172template <
typename Po
intT,
typename NormalT>
void
175 theta_threshold_ =
theta;
179template <
typename Po
intT,
typename NormalT>
float
182 return (residual_threshold_);
186template <
typename Po
intT,
typename NormalT>
void
193template <
typename Po
intT,
typename NormalT>
float
196 return (curvature_threshold_);
200template <
typename Po
intT,
typename NormalT>
void
203 curvature_threshold_ = curvature;
207template <
typename Po
intT,
typename NormalT>
unsigned int
210 return (neighbour_number_);
214template <
typename Po
intT,
typename NormalT>
void
228template <
typename Po
intT,
typename NormalT>
void
242template <
typename Po
intT,
typename NormalT>
void
249template <
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;
273 findPointNeighbours ();
274 applySmoothRegionGrowingAlgorithm ();
279 for (
const auto& cluster : clusters_)
281 if ((cluster.indices.size () >= min_pts_per_cluster_) &&
282 (cluster.indices.size () <= max_pts_per_cluster_))
296template <
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_);
342template <
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)
375template <
typename Po
intT,
typename NormalT>
void
378 int num_of_pts =
static_cast<int> (indices_->size ());
379 point_labels_.resize (input_->size (), -1);
382 std::pair<float, int> pair;
385 if (normal_flag_ ==
true)
421 if (point_labels_[index] == -1)
432template <
typename Po
intT,
typename NormalT>
int
435 std::queue<int>
seeds;
441 while (!
seeds.empty ())
451 if (point_labels_[index] != -1)
482template <
typename Po
intT,
typename NormalT>
bool
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));
509 Eigen::Map<Eigen::Vector3f>
nghbr_normal (
static_cast<float*
> ((*normals_)[
nghbr].normal));
517 if (curvature_flag_ && (*normals_)[
nghbr].curvature > curvature_threshold_)
531 if (residual_flag_ &&
residual > residual_threshold_)
538template <
typename Po
intT,
typename NormalT>
void
569template <
typename Po
intT,
typename NormalT>
void
583 for (
const auto& point : (*indices_))
592 if (clusters_.empty ())
594 point_neighbours_.clear ();
595 point_labels_.clear ();
596 num_pts_in_segment_.clear ();
597 number_of_segments_ = 0;
606 findPointNeighbours ();
607 applySmoothRegionGrowingAlgorithm ();
635 if (!clusters_.empty ())
639 srand (
static_cast<unsigned int> (time (
nullptr)));
640 std::vector<unsigned char> colors;
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));
651 for (
const auto&
i_point: *input_)
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];
685 if (!clusters_.empty ())
689 srand (
static_cast<unsigned int> (time (
nullptr)));
690 std::vector<unsigned char> colors;
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));
701 for (
const auto&
i_point: *input_)
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];
730#define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
shared_ptr< PointCloud< PointT > > Ptr
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.
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.
~RegionGrowing()
This destructor destroys the cloud, normals and search method used for finding KNN.
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.
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.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing Euclidean xyz coordinates, and the RGB color.