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> 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>
94 template <
typename Po
intT,
typename NormalT>
int 101 template <
typename Po
intT,
typename NormalT>
void 108 template <
typename Po
intT,
typename NormalT>
int 115 template <
typename Po
intT,
typename NormalT>
void 122 template <
typename Po
intT,
typename NormalT>
bool 129 template <
typename Po
intT,
typename NormalT>
void 136 template <
typename Po
intT,
typename NormalT>
bool 143 template <
typename Po
intT,
typename NormalT>
void 153 template <
typename Po
intT,
typename NormalT>
bool 160 template <
typename Po
intT,
typename NormalT>
void 170 template <
typename Po
intT,
typename NormalT>
float 177 template <
typename Po
intT,
typename NormalT>
void 184 template <
typename Po
intT,
typename NormalT>
float 191 template <
typename Po
intT,
typename NormalT>
void 198 template <
typename Po
intT,
typename NormalT>
float 205 template <
typename Po
intT,
typename NormalT>
void 212 template <
typename Po
intT,
typename NormalT>
unsigned int 219 template <
typename Po
intT,
typename NormalT>
void 233 template <
typename Po
intT,
typename NormalT>
void 250 template <
typename Po
intT,
typename NormalT>
void 260 template <
typename Po
intT,
typename NormalT>
void 271 if ( !segmentation_is_possible )
278 if ( !segmentation_is_possible )
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++)
295 *cluster_iter_input = *cluster_iter;
296 cluster_iter_input++;
300 clusters_ = std::vector<pcl::PointIndices> (clusters.begin (), cluster_iter_input);
307 template <
typename Po
intT,
typename NormalT>
bool 311 if (
input_->points.size () == 0 )
343 PCL_ERROR (
"[pcl::RegionGrowing::prepareForSegmentation] Empty given indices!\n");
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;
363 for (
int i_point = 0; i_point < point_number; i_point++)
365 int point_index = (*indices_)[i_point];
373 for (
int i_point = 0; i_point < point_number; i_point++)
376 int point_index = (*indices_)[i_point];
386 template <
typename Po
intT,
typename NormalT>
void 389 int num_of_pts =
static_cast<int> (
indices_->size ());
392 std::vector< std::pair<float, int> > point_residual;
393 std::pair<float, int> pair;
394 point_residual.resize (num_of_pts, pair);
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;
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;
442 template <
typename Po
intT,
typename NormalT>
int 445 std::queue<int> seeds;
446 seeds.push (initial_seed);
449 int num_pts_in_segment = 1;
451 while (!seeds.empty ())
454 curr_seed = seeds.front ();
467 bool is_a_seed =
false;
468 bool belongs_to_segment =
validatePoint (initial_seed, curr_seed, index, is_a_seed);
470 if (belongs_to_segment ==
false)
477 num_pts_in_segment++;
488 return (num_pts_in_segment);
492 template <
typename Po
intT,
typename NormalT>
bool 500 data[0] =
input_->points[point].data[0];
501 data[1] =
input_->points[point].data[1];
502 data[2] =
input_->points[point].data[2];
503 data[3] =
input_->points[point].data[3];
504 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
505 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (
normals_->points[point].normal));
510 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (
normals_->points[nghbr].normal));
511 float dot_product = fabsf (nghbr_normal.dot (initial_normal));
512 if (dot_product < cosine_threshold)
519 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (
normals_->points[nghbr].normal));
520 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (
normals_->points[initial_seed].normal));
521 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
522 if (dot_product < cosine_threshold)
535 data_1[0] =
input_->points[nghbr].data[0];
536 data_1[1] =
input_->points[nghbr].data[1];
537 data_1[2] =
input_->points[nghbr].data[2];
538 data_1[3] =
input_->points[nghbr].data[3];
539 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_1));
540 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
548 template <
typename Po
intT,
typename NormalT>
void 552 int number_of_points =
static_cast<int> (
input_->points.size ());
555 clusters_.resize (number_of_segments, segment);
557 for (
int i_seg = 0; i_seg < number_of_segments; i_seg++)
562 std::vector<int> counter;
563 counter.resize (number_of_segments, 0);
565 for (
int i_point = 0; i_point < number_of_points; i_point++)
568 if (segment_index != -1)
570 int point_index = counter[segment_index];
571 clusters_[segment_index].indices[point_index] = i_point;
572 counter[segment_index] = point_index + 1;
580 template <
typename Po
intT,
typename NormalT>
void 586 if ( !segmentation_is_possible )
593 bool point_was_found =
false;
594 int number_of_points = static_cast <
int> (
indices_->size ());
595 for (
int point = 0; point < number_of_points; point++)
598 point_was_found =
true;
612 if ( !segmentation_is_possible )
624 std::vector <pcl::PointIndices>::iterator i_segment;
627 bool segment_was_found =
false;
628 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
630 if (i_segment->indices[i_point] == index)
632 segment_was_found =
true;
634 cluster.
indices.reserve (i_segment->indices.size ());
635 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
639 if (segment_was_found)
659 srand (static_cast<unsigned int> (time (0)));
660 std::vector<unsigned char> colors;
661 for (
size_t i_segment = 0; i_segment <
clusters_.size (); i_segment++)
663 colors.push_back (static_cast<unsigned char> (rand () % 256));
664 colors.push_back (static_cast<unsigned char> (rand () % 256));
665 colors.push_back (static_cast<unsigned char> (rand () % 256));
671 for (
size_t i_point = 0; i_point <
input_->points.size (); i_point++)
674 point.x = *(
input_->points[i_point].data);
675 point.y = *(
input_->points[i_point].data + 1);
676 point.z = *(
input_->points[i_point].data + 2);
680 colored_cloud->
points.push_back (point);
683 std::vector< pcl::PointIndices >::iterator i_segment;
687 std::vector<int>::iterator i_point;
688 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
692 colored_cloud->
points[index].r = colors[3 * next_color];
693 colored_cloud->
points[index].g = colors[3 * next_color + 1];
694 colored_cloud->
points[index].b = colors[3 * next_color + 2];
700 return (colored_cloud);
713 srand (static_cast<unsigned int> (time (0)));
714 std::vector<unsigned char> colors;
715 for (
size_t i_segment = 0; i_segment <
clusters_.size (); i_segment++)
717 colors.push_back (static_cast<unsigned char> (rand () % 256));
718 colors.push_back (static_cast<unsigned char> (rand () % 256));
719 colors.push_back (static_cast<unsigned char> (rand () % 256));
725 for (
size_t i_point = 0; i_point <
input_->points.size (); i_point++)
728 point.x = *(
input_->points[i_point].data);
729 point.y = *(
input_->points[i_point].data + 1);
730 point.z = *(
input_->points[i_point].data + 2);
735 colored_cloud->
points.push_back (point);
738 std::vector< pcl::PointIndices >::iterator i_segment;
742 std::vector<int>::iterator i_point;
743 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
747 colored_cloud->
points[index].r = colors[3 * next_color];
748 colored_cloud->
points[index].g = colors[3 * next_color + 1];
749 colored_cloud->
points[index].b = colors[3 * next_color + 2];
755 return (colored_cloud);
758 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>; 760 #endif // PCL_SEGMENTATION_REGION_GROWING_HPP_ float theta_threshold_
Thershold used for testing the smoothness between points.
float residual_threshold_
Thershold used in residual test.
float curvature_threshold_
Thershold used in curvature test.
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.
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...
std::vector< int > num_pts_in_segment_
Tells how much points each segment contains.
unsigned int neighbour_number_
Number of neighbours to find.
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...
IndicesPtr indices_
A pointer to the vector of point indices to use.
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.
bool residual_flag_
If set to true then residual test will be done during segmentation.
void setCurvatureThreshold(float curvature)
Allows to set curvature threshold used for testing the points.
std::vector< int > indices
int min_pts_per_cluster_
Stores the minimum number of points that a cluster needs to contain in order to be considered valid...
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.
uint32_t height
The point cloud height (if organized as an image-structure).
int number_of_segments_
Stores the number of segments.
bool initCompute()
This method should get called before starting the actual computation.
NormalPtr normals_
Contains normals of the points that will be segmented.
boost::shared_ptr< PointCloud< PointT > > Ptr
float getSmoothnessThreshold() const
Returns smoothness threshold.
int max_pts_per_cluster_
Stores the maximum number of points that a cluster needs to contain in order to be considered valid...
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.
std::vector< pcl::PointIndices > clusters_
After the segmentation it will contain the segments.
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...
bool deinitCompute()
This method should get called after finishing the actual computation.
bool getResidualTestFlag() const
Returns the flag that signalize if the residual test is turned on/off.
std::vector< int > point_labels_
Point labels that tells to which segment each point belongs.
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...
bool smooth_mode_flag_
Flag that signalizes if the smoothness constraint will be used.
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).
bool normal_flag_
If set to true then normal/smoothness test will be done during segmentation.
KdTreePtr search_
Serch method that will be used for KNN.
PointCloudConstPtr input_
The input point cloud dataset.
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.
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
std::vector< std::vector< int > > point_neighbours_
Contains neighbours of each point.