40 #ifndef PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_ 41 #define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_ 43 #include <pcl/segmentation/region_growing_rgb.h> 44 #include <pcl/search/search.h> 45 #include <pcl/search/kdtree.h> 50 template <
typename Po
intT,
typename NormalT>
52 color_p2p_threshold_ (1225.0f),
53 color_r2r_threshold_ (10.0f),
54 distance_threshold_ (0.05f),
55 region_neighbour_number_ (100),
57 segment_neighbours_ (0),
58 segment_distances_ (0),
68 template <
typename Po
intT,
typename NormalT>
78 template <
typename Po
intT,
typename NormalT>
float 85 template <
typename Po
intT,
typename NormalT>
void 92 template <
typename Po
intT,
typename NormalT>
float 99 template <
typename Po
intT,
typename NormalT>
void 106 template <
typename Po
intT,
typename NormalT>
float 113 template <
typename Po
intT,
typename NormalT>
void 120 template <
typename Po
intT,
typename NormalT>
unsigned int 127 template <
typename Po
intT,
typename NormalT>
void 134 template <
typename Po
intT,
typename NormalT>
bool 141 template <
typename Po
intT,
typename NormalT>
void 148 template <
typename Po
intT,
typename NormalT>
void 155 template <
typename Po
intT,
typename NormalT>
void 162 template <
typename Po
intT,
typename NormalT>
void 177 if ( !segmentation_is_possible )
184 if ( !segmentation_is_possible )
197 std::vector<pcl::PointIndices>::iterator cluster_iter =
clusters_.begin ();
203 cluster_iter =
clusters_.erase (cluster_iter);
216 template <
typename Po
intT,
typename NormalT>
bool 220 if (
input_->points.size () == 0 )
261 PCL_ERROR (
"[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
271 template <
typename Po
intT,
typename NormalT>
void 274 int point_number =
static_cast<int> (
indices_->size ());
275 std::vector<int> neighbours;
276 std::vector<float> distances;
281 for (
int i_point = 0; i_point < point_number; i_point++)
283 int point_index = (*indices_)[i_point];
293 template <
typename Po
intT,
typename NormalT>
void 296 std::vector<int> neighbours;
297 std::vector<float> distances;
303 std::vector<int> nghbrs;
304 std::vector<float> dist;
312 template <
typename Po
intT,
typename NormalT>
void 315 std::vector<float> distances;
316 float max_dist = std::numeric_limits<float>::max ();
317 distances.resize (
clusters_.size (), max_dist);
321 for (
int i_point = 0; i_point < number_of_points; i_point++)
323 int point_index =
clusters_[index].indices[i_point];
324 int number_of_neighbours =
static_cast<int> (
point_neighbours_[point_index].size ());
327 for (
int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
330 int segment_index = -1;
333 if ( segment_index != index )
342 std::priority_queue<std::pair<float, int> > segment_neighbours;
345 if (distances[i_seg] < max_dist)
347 segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) );
348 if (
int (segment_neighbours.size ()) > nghbr_number)
349 segment_neighbours.pop ();
353 int size = std::min<int> (
static_cast<int> (segment_neighbours.size ()), nghbr_number);
354 nghbrs.resize (size, 0);
355 dist.resize (size, 0);
357 while ( !segment_neighbours.empty () && counter < nghbr_number )
359 dist[counter] = segment_neighbours.top ().first;
360 nghbrs[counter] = segment_neighbours.top ().second;
361 segment_neighbours.pop ();
367 template <
typename Po
intT,
typename NormalT>
void 370 int number_of_points =
static_cast<int> (
indices_->size ());
373 std::vector< std::vector<unsigned int> > segment_color;
374 std::vector<unsigned int> color;
378 for (
int i_point = 0; i_point < number_of_points; i_point++)
380 int point_index = (*indices_)[i_point];
382 segment_color[segment_index][0] +=
input_->points[point_index].r;
383 segment_color[segment_index][1] +=
input_->points[point_index].g;
384 segment_color[segment_index][2] +=
input_->points[point_index].b;
388 segment_color[i_seg][0] =
static_cast<unsigned int> (
static_cast<float> (segment_color[i_seg][0]) / static_cast<float> (
num_pts_in_segment_[i_seg]));
389 segment_color[i_seg][1] =
static_cast<unsigned int> (
static_cast<float> (segment_color[i_seg][1]) / static_cast<float> (
num_pts_in_segment_[i_seg]));
390 segment_color[i_seg][2] =
static_cast<unsigned int> (
static_cast<float> (segment_color[i_seg][2]) / static_cast<float> (
num_pts_in_segment_[i_seg]));
395 std::vector<unsigned int> num_pts_in_homogeneous_region;
396 std::vector<int> num_seg_in_homogeneous_region;
401 int homogeneous_region_number = 0;
402 int curr_homogeneous_region = 0;
405 curr_homogeneous_region = 0;
409 curr_homogeneous_region = homogeneous_region_number;
411 num_seg_in_homogeneous_region.push_back (1);
412 homogeneous_region_number++;
417 unsigned int i_nghbr = 0;
433 num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
440 segment_color.clear ();
443 std::vector< std::vector<int> > final_segments;
444 std::vector<int> region;
445 final_segments.resize (homogeneous_region_number, region);
446 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
448 final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
451 std::vector<int> counter;
452 counter.resize (homogeneous_region_number, 0);
456 final_segments[ index ][ counter[index] ] = i_seg;
460 std::vector< std::vector< std::pair<float, int> > > region_neighbours;
463 int final_segment_number = homogeneous_region_number;
464 for (
int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
468 if ( region_neighbours[i_reg].empty () )
470 int nearest_neighbour = region_neighbours[i_reg][0].second;
471 if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
474 int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
475 for (
int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
477 int segment_index = final_segments[i_reg][i_seg];
478 final_segments[reg_index].push_back (segment_index);
481 final_segments[i_reg].clear ();
482 num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
483 num_pts_in_homogeneous_region[i_reg] = 0;
484 num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
485 num_seg_in_homogeneous_region[i_reg] = 0;
486 final_segment_number -= 1;
488 int nghbr_number =
static_cast<int> (region_neighbours[reg_index].size ());
489 for (
int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
491 if (
segment_labels_[ region_neighbours[reg_index][i_nghbr].second ] == reg_index )
493 region_neighbours[reg_index][i_nghbr].first = std::numeric_limits<float>::max ();
494 region_neighbours[reg_index][i_nghbr].second = 0;
497 nghbr_number =
static_cast<int> (region_neighbours[i_reg].size ());
498 for (
int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
500 if (
segment_labels_[ region_neighbours[i_reg][i_nghbr].second ] != reg_index )
502 std::pair<float, int> pair;
503 pair.first = region_neighbours[i_reg][i_nghbr].first;
504 pair.second = region_neighbours[i_reg][i_nghbr].second;
505 region_neighbours[reg_index].push_back (pair);
508 region_neighbours[i_reg].clear ();
509 std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (),
comparePair);
513 assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ()));
515 number_of_segments_ = final_segment_number;
519 template <
typename Po
intT,
typename NormalT>
float 522 float difference = 0.0f;
523 difference += float ((first_color[0] - second_color[0]) * (first_color[0] - second_color[0]));
524 difference += float ((first_color[1] - second_color[1]) * (first_color[1] - second_color[1]));
525 difference += float ((first_color[2] - second_color[2]) * (first_color[2] - second_color[2]));
530 template <
typename Po
intT,
typename NormalT>
void 533 int region_number =
static_cast<int> (regions_in.size ());
534 neighbours_out.clear ();
535 neighbours_out.resize (region_number);
537 for (
int i_reg = 0; i_reg < region_number; i_reg++)
539 int segment_num =
static_cast<int> (regions_in[i_reg].size ());
541 for (
int i_seg = 0; i_seg < segment_num; i_seg++)
543 int curr_segment = regions_in[i_reg][i_seg];
545 std::pair<float, int> pair;
546 for (
int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
549 if (
segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
554 pair.second = segment_index;
555 neighbours_out[i_reg].push_back (pair);
559 std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (),
comparePair);
564 template <
typename Po
intT,
typename NormalT>
void 570 for (
int i_seg = 0; i_seg < num_regions; i_seg++)
572 clusters_[i_seg].indices.resize (num_pts_in_region[i_seg]);
575 std::vector<int> counter;
576 counter.resize (num_regions, 0);
577 int point_number =
static_cast<int> (
indices_->size ());
578 for (
int i_point = 0; i_point < point_number; i_point++)
580 int point_index = (*indices_)[i_point];
583 clusters_[index].indices[ counter[index] ] = point_index;
591 std::vector<pcl::PointIndices>::iterator itr1, itr2;
597 while (!(itr1->indices.empty ()) && itr1 < itr2)
599 while ( itr2->indices.empty () && itr1 < itr2)
603 itr1->indices.swap (itr2->indices);
606 if (itr2->indices.empty ())
611 template <
typename Po
intT,
typename NormalT>
bool 617 std::vector<unsigned int> point_color;
618 point_color.resize (3, 0);
619 std::vector<unsigned int> nghbr_color;
620 nghbr_color.resize (3, 0);
621 point_color[0] =
input_->points[point].r;
622 point_color[1] =
input_->points[point].g;
623 point_color[2] =
input_->points[point].b;
624 nghbr_color[0] =
input_->points[nghbr].r;
625 nghbr_color[1] =
input_->points[nghbr].g;
626 nghbr_color[2] =
input_->points[nghbr].b;
637 data[0] =
input_->points[point].data[0];
638 data[1] =
input_->points[point].data[1];
639 data[2] =
input_->points[point].data[2];
640 data[3] =
input_->points[point].data[3];
642 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
643 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (
normals_->points[point].normal));
646 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (
normals_->points[nghbr].normal));
647 float dot_product = fabsf (nghbr_normal.dot (initial_normal));
648 if (dot_product < cosine_threshold)
653 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (
normals_->points[nghbr].normal));
654 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (
normals_->points[initial_seed].normal));
655 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
656 if (dot_product < cosine_threshold)
669 data_p[0] =
input_->points[point].data[0];
670 data_p[1] =
input_->points[point].data[1];
671 data_p[2] =
input_->points[point].data[2];
672 data_p[3] =
input_->points[point].data[3];
674 data_n[0] =
input_->points[nghbr].data[0];
675 data_n[1] =
input_->points[nghbr].data[1];
676 data_n[2] =
input_->points[nghbr].data[2];
677 data_n[3] =
input_->points[nghbr].data[3];
678 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
679 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
680 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (
normals_->points[point].normal));
681 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
690 template <
typename Po
intT,
typename NormalT>
void 696 if ( !segmentation_is_possible )
703 bool point_was_found =
false;
704 int number_of_points = static_cast <
int> (
indices_->size ());
705 for (
int point = 0; point < number_of_points; point++)
708 point_was_found =
true;
727 if ( !segmentation_is_possible )
742 std::vector <pcl::PointIndices>::iterator i_segment;
745 bool segment_was_found =
false;
746 for (
size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
748 if (i_segment->indices[i_point] == index)
750 segment_was_found =
true;
752 cluster.
indices.reserve (i_segment->indices.size ());
753 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.
indices));
757 if (segment_was_found)
767 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_ float theta_threshold_
Thershold used for testing the smoothness between points.
float distance_threshold_
Threshold that tells which points we need to assume neighbouring.
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
float residual_threshold_
Thershold used in residual test.
float curvature_threshold_
Thershold used in curvature test.
float getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
RegionGrowingRGB()
Constructor that sets default values for member variables.
virtual ~RegionGrowingRGB()
Destructor that frees memory.
std::vector< std::vector< float > > segment_distances_
Stores distances for the segment neighbours from segment_neighbours_.
unsigned int region_neighbour_number_
Number of neighbouring segments to find.
std::vector< int > num_pts_in_segment_
Tells how much points each segment contains.
unsigned int neighbour_number_
Number of neighbours to find.
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. ...
IndicesPtr indices_
A pointer to the vector of point indices to use.
bool residual_flag_
If set to true then residual test will be done during segmentation.
std::vector< int > indices
void setNumberOfRegionNeighbours(unsigned int nghbr_number)
This method allows to set the number of neighbours that is used for finding neighbouring segments...
std::vector< std::vector< float > > point_distances_
Stores distances for the point neighbours from point_neighbours_.
int min_pts_per_cluster_
Stores the minimum number of points that a cluster needs to contain in order to be considered valid...
virtual void setResidualTestFlag(bool value)
Allows to turn on/off the residual test.
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.
void findRegionsKNN(int index, int nghbr_number, std::vector< int > &nghbrs, std::vector< float > &dist)
This method finds K nearest neighbours of the given segment.
int max_pts_per_cluster_
Stores the maximum number of points that a cluster needs to contain in order to be considered valid...
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use...
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
float calculateColorimetricalDifference(std::vector< unsigned int > &first_color, std::vector< unsigned int > &second_color) const
This method calculates the colorimetrical difference between two points.
std::vector< std::vector< int > > segment_neighbours_
Stores the neighboures for the corresponding segments.
std::vector< pcl::PointIndices > clusters_
After the segmentation it will contain the segments.
float getDistanceThreshold() const
Returns the distance threshold.
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
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.
std::vector< int > segment_labels_
Stores new indices for segments that were obtained at the region growing stage.
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.
float color_r2r_threshold_
Thershold used in color test for regions.
void setDistanceThreshold(float thresh)
Allows to set distance threshold.
void findRegionNeighbours(std::vector< std::vector< std::pair< float, int > > > &neighbours_out, std::vector< std::vector< int > > ®ions_in)
This method assembles the array containing neighbours of each homogeneous region. ...
virtual void setCurvatureTestFlag(bool value)
Allows to turn on/off the curvature test.
void assembleRegions()
This function simply assembles the regions from list of point labels.
virtual void findPointNeighbours()
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
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.
void setNormalTestFlag(bool value)
Allows to turn on/off the smoothness test.
virtual bool prepareForSegmentation()
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
virtual void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
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...
float color_p2p_threshold_
Thershold used in color test for points.
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.
std::vector< std::vector< int > > point_neighbours_
Contains neighbours of each point.