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> 48 #include <pcl/point_types.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)
533 data[0] =
input_->points[nghbr].data[0];
534 data[1] =
input_->points[nghbr].data[1];
535 data[2] =
input_->points[nghbr].data[2];
536 data[3] =
input_->points[nghbr].data[3];
537 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data));
538 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
546 template <
typename Po
intT,
typename NormalT>
void 550 int number_of_points =
static_cast<int> (
input_->points.size ());
553 clusters_.resize (number_of_segments, segment);
555 for (
int i_seg = 0; i_seg < number_of_segments; i_seg++)
560 std::vector<int> counter;
561 counter.resize (number_of_segments, 0);
563 for (
int i_point = 0; i_point < number_of_points; i_point++)
566 if (segment_index != -1)
568 int point_index = counter[segment_index];
569 clusters_[segment_index].indices[point_index] = i_point;
570 counter[segment_index] = point_index + 1;
578 template <
typename Po
intT,
typename NormalT>
void 584 if ( !segmentation_is_possible )
591 bool point_was_found =
false;
592 int number_of_points = static_cast <
int> (
indices_->size ());
593 for (
int point = 0; point < number_of_points; point++)
596 point_was_found =
true;
610 if ( !segmentation_is_possible )
622 std::vector <pcl::PointIndices>::iterator i_segment;
625 bool segment_was_found =
false;
626 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
628 if (i_segment->indices[i_point] == index)
630 segment_was_found =
true;
632 cluster.
indices.reserve (i_segment->indices.size ());
633 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
637 if (segment_was_found)
657 srand (static_cast<unsigned int> (time (0)));
658 std::vector<unsigned char> colors;
659 for (
size_t i_segment = 0; i_segment <
clusters_.size (); i_segment++)
661 colors.push_back (static_cast<unsigned char> (rand () % 256));
662 colors.push_back (static_cast<unsigned char> (rand () % 256));
663 colors.push_back (static_cast<unsigned char> (rand () % 256));
669 for (
size_t i_point = 0; i_point <
input_->points.size (); i_point++)
672 point.x = *(
input_->points[i_point].data);
673 point.y = *(
input_->points[i_point].data + 1);
674 point.z = *(
input_->points[i_point].data + 2);
678 colored_cloud->
points.push_back (point);
681 std::vector< pcl::PointIndices >::iterator i_segment;
685 std::vector<int>::iterator i_point;
686 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
690 colored_cloud->
points[index].r = colors[3 * next_color];
691 colored_cloud->
points[index].g = colors[3 * next_color + 1];
692 colored_cloud->
points[index].b = colors[3 * next_color + 2];
698 return (colored_cloud);
711 srand (static_cast<unsigned int> (time (0)));
712 std::vector<unsigned char> colors;
713 for (
size_t i_segment = 0; i_segment <
clusters_.size (); i_segment++)
715 colors.push_back (static_cast<unsigned char> (rand () % 256));
716 colors.push_back (static_cast<unsigned char> (rand () % 256));
717 colors.push_back (static_cast<unsigned char> (rand () % 256));
723 for (
size_t i_point = 0; i_point <
input_->points.size (); i_point++)
726 point.x = *(
input_->points[i_point].data);
727 point.y = *(
input_->points[i_point].data + 1);
728 point.z = *(
input_->points[i_point].data + 2);
733 colored_cloud->
points.push_back (point);
736 std::vector< pcl::PointIndices >::iterator i_segment;
740 std::vector<int>::iterator i_point;
741 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++)
745 colored_cloud->
points[index].r = colors[3 * next_color];
746 colored_cloud->
points[index].g = colors[3 * next_color + 1];
747 colored_cloud->
points[index].b = colors[3 * next_color + 2];
753 return (colored_cloud);
756 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>; 758 #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.
float getResidualThreshold() const
Returns residual threshold.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
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...
bool getSmoothModeFlag() const
Returns the flag value.
std::vector< int > num_pts_in_segment_
Tells how much points each segment contains.
unsigned int neighbour_number_
Number of neighbours to find.
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
unsigned int getNumberOfNeighbours() const
Returns the number of nearest neighbours used for KNN.
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.
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.
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
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.
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.
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.
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.
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...
NormalPtr getInputNormals() const
Returns normals.
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).
KdTreePtr getSearchMethod() const
Returns the pointer to the search method that is used for KNN.
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.
bool getResidualTestFlag() const
Returns the flag that signalize if the residual test is turned on/off.
PointCloudConstPtr input_
The input point cloud dataset.
float getSmoothnessThreshold() const
Returns smoothness threshold.
virtual void setCurvatureTestFlag(bool value)
Allows to turn on/off the curvature test.
float getCurvatureThreshold() const
Returns curvature threshold.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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 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.