Point Cloud Library (PCL)  1.10.0
region_growing_rgb.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the copyright holder(s) nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author : Sergey Ushakov
36  * Email : mine_all_mine@bk.ru
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
41 #define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
42 
43 #include <pcl/segmentation/region_growing_rgb.h>
44 #include <pcl/search/search.h>
45 #include <pcl/search/kdtree.h>
46 
47 #include <queue>
48 
49 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointT, typename NormalT>
52  color_p2p_threshold_ (1225.0f),
53  color_r2r_threshold_ (10.0f),
54  distance_threshold_ (0.05f),
55  region_neighbour_number_ (100),
56  point_distances_ (0),
57  segment_neighbours_ (0),
58  segment_distances_ (0),
59  segment_labels_ (0)
60 {
61  normal_flag_ = false;
62  curvature_flag_ = false;
63  residual_flag_ = false;
65 }
66 
67 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointT, typename NormalT>
70 {
71  point_distances_.clear ();
72  segment_neighbours_.clear ();
73  segment_distances_.clear ();
74  segment_labels_.clear ();
75 }
76 
77 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78 template <typename PointT, typename NormalT> float
80 {
81  return (powf (color_p2p_threshold_, 0.5f));
82 }
83 
84 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
85 template <typename PointT, typename NormalT> void
87 {
88  color_p2p_threshold_ = thresh * thresh;
89 }
90 
91 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
92 template <typename PointT, typename NormalT> float
94 {
95  return (powf (color_r2r_threshold_, 0.5f));
96 }
97 
98 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
99 template <typename PointT, typename NormalT> void
101 {
102  color_r2r_threshold_ = thresh * thresh;
103 }
104 
105 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
106 template <typename PointT, typename NormalT> float
108 {
109  return (powf (distance_threshold_, 0.5f));
110 }
111 
112 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
113 template <typename PointT, typename NormalT> void
115 {
116  distance_threshold_ = thresh * thresh;
117 }
118 
119 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
120 template <typename PointT, typename NormalT> unsigned int
122 {
123  return (region_neighbour_number_);
124 }
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
127 template <typename PointT, typename NormalT> void
129 {
130  region_neighbour_number_ = nghbr_number;
131 }
132 
133 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
134 template <typename PointT, typename NormalT> bool
136 {
137  return (normal_flag_);
138 }
139 
140 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
141 template <typename PointT, typename NormalT> void
143 {
144  normal_flag_ = value;
145 }
146 
147 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
148 template <typename PointT, typename NormalT> void
150 {
151  curvature_flag_ = value;
152 }
153 
154 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
155 template <typename PointT, typename NormalT> void
157 {
158  residual_flag_ = value;
159 }
160 
161 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
162 template <typename PointT, typename NormalT> void
163 pcl::RegionGrowingRGB<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
164 {
165  clusters_.clear ();
166  clusters.clear ();
167  point_neighbours_.clear ();
168  point_labels_.clear ();
169  num_pts_in_segment_.clear ();
170  point_distances_.clear ();
171  segment_neighbours_.clear ();
172  segment_distances_.clear ();
173  segment_labels_.clear ();
174  number_of_segments_ = 0;
175 
176  bool segmentation_is_possible = initCompute ();
177  if ( !segmentation_is_possible )
178  {
179  deinitCompute ();
180  return;
181  }
182 
183  segmentation_is_possible = prepareForSegmentation ();
184  if ( !segmentation_is_possible )
185  {
186  deinitCompute ();
187  return;
188  }
189 
190  findPointNeighbours ();
191  applySmoothRegionGrowingAlgorithm ();
193 
194  findSegmentNeighbours ();
195  applyRegionMergingAlgorithm ();
196 
197  std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin ();
198  while (cluster_iter != clusters_.end ())
199  {
200  if (static_cast<int> (cluster_iter->indices.size ()) < min_pts_per_cluster_ ||
201  static_cast<int> (cluster_iter->indices.size ()) > max_pts_per_cluster_)
202  {
203  cluster_iter = clusters_.erase (cluster_iter);
204  }
205  else
206  cluster_iter++;
207  }
208 
209  clusters.reserve (clusters_.size ());
210  std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
211 
212  deinitCompute ();
213 }
214 
215 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
216 template <typename PointT, typename NormalT> bool
218 {
219  // if user forgot to pass point cloud or if it is empty
220  if ( input_->points.empty () )
221  return (false);
222 
223  // if normal/smoothness test is on then we need to check if all needed variables and parameters
224  // were correctly initialized
225  if (normal_flag_)
226  {
227  // if user forgot to pass normals or the sizes of point and normal cloud are different
228  if ( !normals_ || input_->points.size () != normals_->points.size () )
229  return (false);
230  }
231 
232  // if residual test is on then we need to check if all needed parameters were correctly initialized
233  if (residual_flag_)
234  {
235  if (residual_threshold_ <= 0.0f)
236  return (false);
237  }
238 
239  // if curvature test is on ...
240  // if (curvature_flag_)
241  // {
242  // in this case we do not need to check anything that related to it
243  // so we simply commented it
244  // }
245 
246  // here we check the parameters related to color-based segmentation
247  if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f )
248  return (false);
249 
250  // from here we check those parameters that are always valuable
251  if (neighbour_number_ == 0)
252  return (false);
253 
254  // if user didn't set search method
255  if (!search_)
256  search_.reset (new pcl::search::KdTree<PointT>);
257 
258  if (indices_)
259  {
260  if (indices_->empty ())
261  PCL_ERROR ("[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
262  search_->setInputCloud (input_, indices_);
263  }
264  else
265  search_->setInputCloud (input_);
266 
267  return (true);
268 }
269 
270 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
271 template <typename PointT, typename NormalT> void
273 {
274  int point_number = static_cast<int> (indices_->size ());
275  std::vector<int> neighbours;
276  std::vector<float> distances;
277 
278  point_neighbours_.resize (input_->points.size (), neighbours);
279  point_distances_.resize (input_->points.size (), distances);
280 
281  for (int i_point = 0; i_point < point_number; i_point++)
282  {
283  int point_index = (*indices_)[i_point];
284  neighbours.clear ();
285  distances.clear ();
286  search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances);
287  point_neighbours_[point_index].swap (neighbours);
288  point_distances_[point_index].swap (distances);
289  }
290 }
291 
292 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
293 template <typename PointT, typename NormalT> void
295 {
296  std::vector<int> neighbours;
297  std::vector<float> distances;
298  segment_neighbours_.resize (number_of_segments_, neighbours);
299  segment_distances_.resize (number_of_segments_, distances);
300 
301  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
302  {
303  std::vector<int> nghbrs;
304  std::vector<float> dist;
305  findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
306  segment_neighbours_[i_seg].swap (nghbrs);
307  segment_distances_[i_seg].swap (dist);
308  }
309 }
310 
311 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
312 template <typename PointT,typename NormalT> void
313 pcl::RegionGrowingRGB<PointT, NormalT>::findRegionsKNN (int index, int nghbr_number, std::vector<int>& nghbrs, std::vector<float>& dist)
314 {
315  std::vector<float> distances;
316  float max_dist = std::numeric_limits<float>::max ();
317  distances.resize (clusters_.size (), max_dist);
318 
319  int number_of_points = num_pts_in_segment_[index];
320  //loop through every point in this segment and check neighbours
321  for (int i_point = 0; i_point < number_of_points; i_point++)
322  {
323  int point_index = clusters_[index].indices[i_point];
324  int number_of_neighbours = static_cast<int> (point_neighbours_[point_index].size ());
325  //loop through every neighbour of the current point, find out to which segment it belongs
326  //and if it belongs to neighbouring segment and is close enough then remember segment and its distance
327  for (int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
328  {
329  // find segment
330  int segment_index = -1;
331  segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
332 
333  if ( segment_index != index )
334  {
335  // try to push it to the queue
336  if (distances[segment_index] > point_distances_[point_index][i_nghbr])
337  distances[segment_index] = point_distances_[point_index][i_nghbr];
338  }
339  }
340  }// next point
341 
342  std::priority_queue<std::pair<float, int> > segment_neighbours;
343  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
344  {
345  if (distances[i_seg] < max_dist)
346  {
347  segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) );
348  if (int (segment_neighbours.size ()) > nghbr_number)
349  segment_neighbours.pop ();
350  }
351  }
352 
353  int size = std::min<int> (static_cast<int> (segment_neighbours.size ()), nghbr_number);
354  nghbrs.resize (size, 0);
355  dist.resize (size, 0);
356  int counter = 0;
357  while ( !segment_neighbours.empty () && counter < nghbr_number )
358  {
359  dist[counter] = segment_neighbours.top ().first;
360  nghbrs[counter] = segment_neighbours.top ().second;
361  segment_neighbours.pop ();
362  counter++;
363  }
364 }
365 
366 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
367 template <typename PointT, typename NormalT> void
369 {
370  int number_of_points = static_cast<int> (indices_->size ());
371 
372  // calculate color of each segment
373  std::vector< std::vector<unsigned int> > segment_color;
374  std::vector<unsigned int> color;
375  color.resize (3, 0);
376  segment_color.resize (number_of_segments_, color);
377 
378  for (int i_point = 0; i_point < number_of_points; i_point++)
379  {
380  int point_index = (*indices_)[i_point];
381  int segment_index = point_labels_[point_index];
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;
385  }
386  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
387  {
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]));
391  }
392 
393  // now it is time to find out if there are segments with a similar color
394  // and merge them together
395  std::vector<unsigned int> num_pts_in_homogeneous_region;
396  std::vector<int> num_seg_in_homogeneous_region;
397 
398  segment_labels_.resize (number_of_segments_, -1);
399 
400  float dist_thresh = distance_threshold_;
401  int homogeneous_region_number = 0;
402  int curr_homogeneous_region = 0;
403  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
404  {
405  curr_homogeneous_region = 0;
406  if (segment_labels_[i_seg] == -1)
407  {
408  segment_labels_[i_seg] = homogeneous_region_number;
409  curr_homogeneous_region = homogeneous_region_number;
410  num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
411  num_seg_in_homogeneous_region.push_back (1);
412  homogeneous_region_number++;
413  }
414  else
415  curr_homogeneous_region = segment_labels_[i_seg];
416 
417  unsigned int i_nghbr = 0;
418  while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
419  {
420  int index = segment_neighbours_[i_seg][i_nghbr];
421  if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
422  {
423  i_nghbr++;
424  continue;
425  }
426  if ( segment_labels_[index] == -1 )
427  {
428  float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
429  if (difference < color_r2r_threshold_)
430  {
431  segment_labels_[index] = curr_homogeneous_region;
432  num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
433  num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
434  }
435  }
436  i_nghbr++;
437  }// next neighbour
438  }// next segment
439 
440  segment_color.clear ();
441  color.clear ();
442 
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++)
447  {
448  final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
449  }
450 
451  std::vector<int> counter;
452  counter.resize (homogeneous_region_number, 0);
453  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
454  {
455  int index = segment_labels_[i_seg];
456  final_segments[ index ][ counter[index] ] = i_seg;
457  counter[index] += 1;
458  }
459 
460  std::vector< std::vector< std::pair<float, int> > > region_neighbours;
461  findRegionNeighbours (region_neighbours, final_segments);
462 
463  int final_segment_number = homogeneous_region_number;
464  for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
465  {
466  if (static_cast<int> (num_pts_in_homogeneous_region[i_reg]) < min_pts_per_cluster_)
467  {
468  if ( region_neighbours[i_reg].empty () )
469  continue;
470  int nearest_neighbour = region_neighbours[i_reg][0].second;
471  if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
472  continue;
473  int reg_index = segment_labels_[nearest_neighbour];
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++)
476  {
477  int segment_index = final_segments[i_reg][i_seg];
478  final_segments[reg_index].push_back (segment_index);
479  segment_labels_[segment_index] = reg_index;
480  }
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;
487 
488  int nghbr_number = static_cast<int> (region_neighbours[reg_index].size ());
489  for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
490  {
491  if ( segment_labels_[ region_neighbours[reg_index][i_nghbr].second ] == reg_index )
492  {
493  region_neighbours[reg_index][i_nghbr].first = std::numeric_limits<float>::max ();
494  region_neighbours[reg_index][i_nghbr].second = 0;
495  }
496  }
497  nghbr_number = static_cast<int> (region_neighbours[i_reg].size ());
498  for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
499  {
500  if ( segment_labels_[ region_neighbours[i_reg][i_nghbr].second ] != reg_index )
501  {
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);
506  }
507  }
508  region_neighbours[i_reg].clear ();
509  std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (), comparePair);
510  }
511  }
512 
513  assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ()));
514 
515  number_of_segments_ = final_segment_number;
516 }
517 
518 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
519 template <typename PointT, typename NormalT> float
520 pcl::RegionGrowingRGB<PointT, NormalT>::calculateColorimetricalDifference (std::vector<unsigned int>& first_color, std::vector<unsigned int>& second_color) const
521 {
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]));
526  return (difference);
527 }
528 
529 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
530 template <typename PointT, typename NormalT> void
531 pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::vector< std::pair<float, int> > >& neighbours_out, std::vector< std::vector<int> >& regions_in)
532 {
533  int region_number = static_cast<int> (regions_in.size ());
534  neighbours_out.clear ();
535  neighbours_out.resize (region_number);
536 
537  for (int i_reg = 0; i_reg < region_number; i_reg++)
538  {
539  int segment_num = static_cast<int> (regions_in[i_reg].size ());
540  neighbours_out[i_reg].reserve (segment_num * region_neighbour_number_);
541  for (int i_seg = 0; i_seg < segment_num; i_seg++)
542  {
543  int curr_segment = regions_in[i_reg][i_seg];
544  int nghbr_number = static_cast<int> (segment_neighbours_[curr_segment].size ());
545  std::pair<float, int> pair;
546  for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
547  {
548  int segment_index = segment_neighbours_[curr_segment][i_nghbr];
549  if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
550  continue;
551  if (segment_labels_[segment_index] != i_reg)
552  {
553  pair.first = segment_distances_[curr_segment][i_nghbr];
554  pair.second = segment_index;
555  neighbours_out[i_reg].push_back (pair);
556  }
557  }// next neighbour
558  }// next segment
559  std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (), comparePair);
560  }// next homogeneous region
561 }
562 
563 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
564 template <typename PointT, typename NormalT> void
565 pcl::RegionGrowingRGB<PointT, NormalT>::assembleRegions (std::vector<unsigned int>& num_pts_in_region, int num_regions)
566 {
567  clusters_.clear ();
568  pcl::PointIndices segment;
569  clusters_.resize (num_regions, segment);
570  for (int i_seg = 0; i_seg < num_regions; i_seg++)
571  {
572  clusters_[i_seg].indices.resize (num_pts_in_region[i_seg]);
573  }
574 
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++)
579  {
580  int point_index = (*indices_)[i_point];
581  int index = point_labels_[point_index];
582  index = segment_labels_[index];
583  clusters_[index].indices[ counter[index] ] = point_index;
584  counter[index] += 1;
585  }
586 
587  // now we need to erase empty regions
588  if (clusters_.empty ())
589  return;
590 
591  std::vector<pcl::PointIndices>::iterator itr1, itr2;
592  itr1 = clusters_.begin ();
593  itr2 = clusters_.end () - 1;
594 
595  while (itr1 < itr2)
596  {
597  while (!(itr1->indices.empty ()) && itr1 < itr2)
598  itr1++;
599  while ( itr2->indices.empty () && itr1 < itr2)
600  itr2--;
601 
602  if (itr1 != itr2)
603  itr1->indices.swap (itr2->indices);
604  }
605 
606  if (itr2->indices.empty ())
607  clusters_.erase (itr2, clusters_.end ());
608 }
609 
610 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
611 template <typename PointT, typename NormalT> bool
612 pcl::RegionGrowingRGB<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const
613 {
614  is_a_seed = true;
615 
616  // check the color difference
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;
627  float difference = calculateColorimetricalDifference (point_color, nghbr_color);
628  if (difference > color_p2p_threshold_)
629  return (false);
630 
631  float cosine_threshold = std::cos (theta_threshold_);
632 
633  // check the angle between normals if needed
634  if (normal_flag_)
635  {
636  float data[4];
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];
641 
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));
644  if (smooth_mode_flag_ == true)
645  {
646  Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
647  float dot_product = std::abs (nghbr_normal.dot (initial_normal));
648  if (dot_product < cosine_threshold)
649  return (false);
650  }
651  else
652  {
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 = std::abs (nghbr_normal.dot (initial_seed_normal));
656  if (dot_product < cosine_threshold)
657  return (false);
658  }
659  }
660 
661  // check the curvature if needed
662  if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
663  is_a_seed = false;
664 
665  // check the residual if needed
666  if (residual_flag_)
667  {
668  float data_p[4];
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];
673  float data_n[4];
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 = std::abs (initial_normal.dot (initial_point - nghbr_point));
682  if (residual > residual_threshold_)
683  is_a_seed = false;
684  }
685 
686  return (true);
687 }
688 
689 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
690 template <typename PointT, typename NormalT> void
692 {
693  cluster.indices.clear ();
694 
695  bool segmentation_is_possible = initCompute ();
696  if ( !segmentation_is_possible )
697  {
698  deinitCompute ();
699  return;
700  }
701 
702  // first of all we need to find out if this point belongs to cloud
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++)
706  if ( (*indices_)[point] == index)
707  {
708  point_was_found = true;
709  break;
710  }
711 
712  if (point_was_found)
713  {
714  if (clusters_.empty ())
715  {
716  clusters_.clear ();
717  point_neighbours_.clear ();
718  point_labels_.clear ();
719  num_pts_in_segment_.clear ();
720  point_distances_.clear ();
721  segment_neighbours_.clear ();
722  segment_distances_.clear ();
723  segment_labels_.clear ();
724  number_of_segments_ = 0;
725 
726  segmentation_is_possible = prepareForSegmentation ();
727  if ( !segmentation_is_possible )
728  {
729  deinitCompute ();
730  return;
731  }
732 
733  findPointNeighbours ();
734  applySmoothRegionGrowingAlgorithm ();
736 
737  findSegmentNeighbours ();
738  applyRegionMergingAlgorithm ();
739  }
740  // if we have already made the segmentation, then find the segment
741  // to which this point belongs
742  for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
743  {
744  bool segment_was_found = false;
745  for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
746  {
747  if (i_segment->indices[i_point] == index)
748  {
749  segment_was_found = true;
750  cluster.indices.clear ();
751  cluster.indices.reserve (i_segment->indices.size ());
752  std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices));
753  break;
754  }
755  }
756  if (segment_was_found)
757  {
758  break;
759  }
760  }// next segment
761  }// end if point was found
762 
763  deinitCompute ();
764 }
765 
766 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
pcl::RegionGrowingRGB::getRegionColorThreshold
float getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
Definition: region_growing_rgb.hpp:93
pcl::RegionGrowingRGB::setNormalTestFlag
void setNormalTestFlag(bool value)
Allows to turn on/off the smoothness test.
Definition: region_growing_rgb.hpp:142
pcl::RegionGrowingRGB::getNormalTestFlag
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
Definition: region_growing_rgb.hpp:135
pcl::RegionGrowing< PointT, pcl::Normal >::normal_flag_
bool normal_flag_
If set to true then normal/smoothness test will be done during segmentation.
Definition: region_growing.h:324
pcl::RegionGrowingRGB::findPointNeighbours
void findPointNeighbours() override
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
Definition: region_growing_rgb.hpp:272
pcl::RegionGrowingRGB::getNumberOfRegionNeighbours
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
Definition: region_growing_rgb.hpp:121
pcl::RegionGrowing< PointT, pcl::Normal >::curvature_flag_
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
Definition: region_growing.h:292
pcl::RegionGrowingRGB::getSegmentFromPoint
void getSegmentFromPoint(int index, pcl::PointIndices &cluster) override
For a given point this function builds a segment to which it belongs and returns this segment.
Definition: region_growing_rgb.hpp:691
pcl::RegionGrowingRGB::setCurvatureTestFlag
void setCurvatureTestFlag(bool value) override
Allows to turn on/off the curvature test.
Definition: region_growing_rgb.hpp:149
pcl::RegionGrowingRGB::setPointColorThreshold
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.
Definition: region_growing_rgb.hpp:86
pcl::RegionGrowingRGB::extract
void extract(std::vector< pcl::PointIndices > &clusters) override
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
Definition: region_growing_rgb.hpp:163
pcl::RegionGrowingRGB::calculateColorimetricalDifference
float calculateColorimetricalDifference(std::vector< unsigned int > &first_color, std::vector< unsigned int > &second_color) const
This method calculates the colorimetrical difference between two points.
Definition: region_growing_rgb.hpp:520
pcl::RegionGrowing< PointT, pcl::Normal >::residual_flag_
bool residual_flag_
If set to true then residual test will be done during segmentation.
Definition: region_growing.h:295
pcl::RegionGrowingRGB::setResidualTestFlag
void setResidualTestFlag(bool value) override
Allows to turn on/off the residual test.
Definition: region_growing_rgb.hpp:156
pcl::RegionGrowingRGB::~RegionGrowingRGB
~RegionGrowingRGB()
Destructor that frees memory.
Definition: region_growing_rgb.hpp:69
pcl::RegionGrowingRGB::prepareForSegmentation
bool prepareForSegmentation() override
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
Definition: region_growing_rgb.hpp:217
pcl::comparePair
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
Definition: region_growing.h:341
pcl::search::KdTree< PointT >
pcl::RegionGrowingRGB::findRegionsKNN
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.
Definition: region_growing_rgb.hpp:313
pcl::RegionGrowing::assembleRegions
void assembleRegions()
This function simply assembles the regions from list of point labels.
Definition: region_growing.hpp:539
pcl::RegionGrowingRGB::findSegmentNeighbours
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use.
Definition: region_growing_rgb.hpp:294
pcl::RegionGrowingRGB::setNumberOfRegionNeighbours
void setNumberOfRegionNeighbours(unsigned int nghbr_number)
This method allows to set the number of neighbours that is used for finding neighbouring segments.
Definition: region_growing_rgb.hpp:128
pcl::PointIndices::indices
std::vector< int > indices
Definition: PointIndices.h:19
pcl::RegionGrowingRGB::setDistanceThreshold
void setDistanceThreshold(float thresh)
Allows to set distance threshold.
Definition: region_growing_rgb.hpp:114
pcl::PointIndices
Definition: PointIndices.h:12
pcl::RegionGrowingRGB::setRegionColorThreshold
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
Definition: region_growing_rgb.hpp:100
pcl::RegionGrowing< PointT, pcl::Normal >::min_pts_per_cluster_
int min_pts_per_cluster_
Stores the minimum number of points that a cluster needs to contain in order to be considered valid.
Definition: region_growing.h:283
pcl::RegionGrowingRGB::getPointColorThreshold
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
Definition: region_growing_rgb.hpp:79
pcl::RegionGrowingRGB::findRegionNeighbours
void findRegionNeighbours(std::vector< std::vector< std::pair< float, int > > > &neighbours_out, std::vector< std::vector< int > > &regions_in)
This method assembles the array containing neighbours of each homogeneous region.
Definition: region_growing_rgb.hpp:531
pcl::RegionGrowingRGB::RegionGrowingRGB
RegionGrowingRGB()
Constructor that sets default values for member variables.
Definition: region_growing_rgb.hpp:51
pcl::RegionGrowingRGB::applyRegionMergingAlgorithm
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
Definition: region_growing_rgb.hpp:368
pcl::RegionGrowingRGB::validatePoint
bool validatePoint(int initial_seed, int point, int nghbr, bool &is_a_seed) const override
This function is checking if the point with index 'nghbr' belongs to the segment.
Definition: region_growing_rgb.hpp:612
pcl::RegionGrowingRGB::getDistanceThreshold
float getDistanceThreshold() const
Returns the distance threshold.
Definition: region_growing_rgb.hpp:107