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;
435 seed_counter = i_seed;
443 template <
typename Po
intT,
typename NormalT>
int 446 std::queue<int> seeds;
447 seeds.push (initial_seed);
450 int num_pts_in_segment = 1;
452 while (!seeds.empty ())
455 curr_seed = seeds.front ();
468 bool is_a_seed =
false;
469 bool belongs_to_segment =
validatePoint (initial_seed, curr_seed, index, is_a_seed);
471 if (belongs_to_segment ==
false)
478 num_pts_in_segment++;
489 return (num_pts_in_segment);
493 template <
typename Po
intT,
typename NormalT>
bool 501 data[0] =
input_->points[point].data[0];
502 data[1] =
input_->points[point].data[1];
503 data[2] =
input_->points[point].data[2];
504 data[3] =
input_->points[point].data[3];
505 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
506 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (
normals_->points[point].normal));
511 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (
normals_->points[nghbr].normal));
512 float dot_product = fabsf (nghbr_normal.dot (initial_normal));
513 if (dot_product < cosine_threshold)
520 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (
normals_->points[nghbr].normal));
521 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (
normals_->points[initial_seed].normal));
522 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
523 if (dot_product < cosine_threshold)
536 data_1[0] =
input_->points[nghbr].data[0];
537 data_1[1] =
input_->points[nghbr].data[1];
538 data_1[2] =
input_->points[nghbr].data[2];
539 data_1[3] =
input_->points[nghbr].data[3];
540 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_1));
541 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
549 template <
typename Po
intT,
typename NormalT>
void 553 int number_of_points =
static_cast<int> (
input_->points.size ());
556 clusters_.resize (number_of_segments, segment);
558 for (
int i_seg = 0; i_seg < number_of_segments; i_seg++)
563 std::vector<int> counter;
564 counter.resize (number_of_segments, 0);
566 for (
int i_point = 0; i_point < number_of_points; i_point++)
569 if (segment_index != -1)
571 int point_index = counter[segment_index];
572 clusters_[segment_index].indices[point_index] = i_point;
573 counter[segment_index] = point_index + 1;
581 template <
typename Po
intT,
typename NormalT>
void 587 if ( !segmentation_is_possible )
594 bool point_was_found =
false;
595 int number_of_points = static_cast <
int> (
indices_->size ());
596 for (
int point = 0; point < number_of_points; point++)
599 point_was_found =
true;
613 if ( !segmentation_is_possible )
625 std::vector <pcl::PointIndices>::iterator i_segment;
628 bool segment_was_found =
false;
629 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
631 if (i_segment->indices[i_point] == index)
633 segment_was_found =
true;
635 cluster.
indices.reserve (i_segment->indices.size ());
636 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
640 if (segment_was_found)
660 srand (static_cast<unsigned int> (time (0)));
661 std::vector<unsigned char> colors;
662 for (
size_t i_segment = 0; i_segment <
clusters_.size (); i_segment++)
664 colors.push_back (static_cast<unsigned char> (rand () % 256));
665 colors.push_back (static_cast<unsigned char> (rand () % 256));
666 colors.push_back (static_cast<unsigned char> (rand () % 256));
672 for (
size_t i_point = 0; i_point <
input_->points.size (); i_point++)
675 point.x = *(
input_->points[i_point].data);
676 point.y = *(
input_->points[i_point].data + 1);
677 point.z = *(
input_->points[i_point].data + 2);
681 colored_cloud->
points.push_back (point);
684 std::vector< pcl::PointIndices >::iterator i_segment;
688 std::vector<int>::iterator i_point;
689 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
693 colored_cloud->
points[index].r = colors[3 * next_color];
694 colored_cloud->
points[index].g = colors[3 * next_color + 1];
695 colored_cloud->
points[index].b = colors[3 * next_color + 2];
701 return (colored_cloud);
714 srand (static_cast<unsigned int> (time (0)));
715 std::vector<unsigned char> colors;
716 for (
size_t i_segment = 0; i_segment <
clusters_.size (); i_segment++)
718 colors.push_back (static_cast<unsigned char> (rand () % 256));
719 colors.push_back (static_cast<unsigned char> (rand () % 256));
720 colors.push_back (static_cast<unsigned char> (rand () % 256));
726 for (
size_t i_point = 0; i_point <
input_->points.size (); i_point++)
729 point.x = *(
input_->points[i_point].data);
730 point.y = *(
input_->points[i_point].data + 1);
731 point.z = *(
input_->points[i_point].data + 2);
736 colored_cloud->
points.push_back (point);
739 std::vector< pcl::PointIndices >::iterator i_segment;
743 std::vector<int>::iterator i_point;
744 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
748 colored_cloud->
points[index].r = colors[3 * next_color];
749 colored_cloud->
points[index].g = colors[3 * next_color + 1];
750 colored_cloud->
points[index].b = colors[3 * next_color + 2];
756 return (colored_cloud);
759 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>; 761 #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 in any of their floating point fields)...
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.