Point Cloud Library (PCL)  1.11.1-dev
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/console/print.h> // for PCL_ERROR
44 #include <pcl/segmentation/region_growing_rgb.h>
45 #include <pcl/search/search.h>
46 #include <pcl/search/kdtree.h>
47 
48 #include <queue>
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointT, typename NormalT>
53  color_p2p_threshold_ (1225.0f),
54  color_r2r_threshold_ (10.0f),
55  distance_threshold_ (0.05f),
56  region_neighbour_number_ (100),
57  point_distances_ (0),
58  segment_neighbours_ (0),
59  segment_distances_ (0),
60  segment_labels_ (0)
61 {
62  normal_flag_ = false;
63  curvature_flag_ = false;
64  residual_flag_ = false;
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69 template <typename PointT, typename NormalT>
71 {
72  point_distances_.clear ();
73  segment_neighbours_.clear ();
74  segment_distances_.clear ();
75  segment_labels_.clear ();
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT, typename NormalT> float
81 {
82  return (powf (color_p2p_threshold_, 0.5f));
83 }
84 
85 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename PointT, typename NormalT> void
88 {
89  color_p2p_threshold_ = thresh * thresh;
90 }
91 
92 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
93 template <typename PointT, typename NormalT> float
95 {
96  return (powf (color_r2r_threshold_, 0.5f));
97 }
98 
99 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
100 template <typename PointT, typename NormalT> void
102 {
103  color_r2r_threshold_ = thresh * thresh;
104 }
105 
106 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
107 template <typename PointT, typename NormalT> float
109 {
110  return (powf (distance_threshold_, 0.5f));
111 }
112 
113 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
114 template <typename PointT, typename NormalT> void
116 {
117  distance_threshold_ = thresh * thresh;
118 }
119 
120 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
121 template <typename PointT, typename NormalT> unsigned int
123 {
124  return (region_neighbour_number_);
125 }
126 
127 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
128 template <typename PointT, typename NormalT> void
130 {
131  region_neighbour_number_ = nghbr_number;
132 }
133 
134 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
135 template <typename PointT, typename NormalT> bool
137 {
138  return (normal_flag_);
139 }
140 
141 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
142 template <typename PointT, typename NormalT> void
144 {
145  normal_flag_ = value;
146 }
147 
148 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
149 template <typename PointT, typename NormalT> void
151 {
152  curvature_flag_ = value;
153 }
154 
155 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
156 template <typename PointT, typename NormalT> void
158 {
159  residual_flag_ = value;
160 }
161 
162 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
163 template <typename PointT, typename NormalT> void
164 pcl::RegionGrowingRGB<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
165 {
166  clusters_.clear ();
167  clusters.clear ();
168  point_neighbours_.clear ();
169  point_labels_.clear ();
170  num_pts_in_segment_.clear ();
171  point_distances_.clear ();
172  segment_neighbours_.clear ();
173  segment_distances_.clear ();
174  segment_labels_.clear ();
175  number_of_segments_ = 0;
176 
177  bool segmentation_is_possible = initCompute ();
178  if ( !segmentation_is_possible )
179  {
180  deinitCompute ();
181  return;
182  }
183 
184  segmentation_is_possible = prepareForSegmentation ();
185  if ( !segmentation_is_possible )
186  {
187  deinitCompute ();
188  return;
189  }
190 
191  findPointNeighbours ();
192  applySmoothRegionGrowingAlgorithm ();
194 
195  findSegmentNeighbours ();
196  applyRegionMergingAlgorithm ();
197 
198  std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin ();
199  while (cluster_iter != clusters_.end ())
200  {
201  if (static_cast<int> (cluster_iter->indices.size ()) < min_pts_per_cluster_ ||
202  static_cast<int> (cluster_iter->indices.size ()) > max_pts_per_cluster_)
203  {
204  cluster_iter = clusters_.erase (cluster_iter);
205  }
206  else
207  ++cluster_iter;
208  }
209 
210  clusters.reserve (clusters_.size ());
211  std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
212 
213  deinitCompute ();
214 }
215 
216 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
217 template <typename PointT, typename NormalT> bool
219 {
220  // if user forgot to pass point cloud or if it is empty
221  if ( input_->points.empty () )
222  return (false);
223 
224  // if normal/smoothness test is on then we need to check if all needed variables and parameters
225  // were correctly initialized
226  if (normal_flag_)
227  {
228  // if user forgot to pass normals or the sizes of point and normal cloud are different
229  if ( !normals_ || input_->size () != normals_->size () )
230  return (false);
231  }
232 
233  // if residual test is on then we need to check if all needed parameters were correctly initialized
234  if (residual_flag_)
235  {
236  if (residual_threshold_ <= 0.0f)
237  return (false);
238  }
239 
240  // if curvature test is on ...
241  // if (curvature_flag_)
242  // {
243  // in this case we do not need to check anything that related to it
244  // so we simply commented it
245  // }
246 
247  // here we check the parameters related to color-based segmentation
248  if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f )
249  return (false);
250 
251  // from here we check those parameters that are always valuable
252  if (neighbour_number_ == 0)
253  return (false);
254 
255  // if user didn't set search method
256  if (!search_)
257  search_.reset (new pcl::search::KdTree<PointT>);
258 
259  if (indices_)
260  {
261  if (indices_->empty ())
262  PCL_ERROR ("[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
263  search_->setInputCloud (input_, indices_);
264  }
265  else
266  search_->setInputCloud (input_);
267 
268  return (true);
269 }
270 
271 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
272 template <typename PointT, typename NormalT> void
274 {
275  int point_number = static_cast<int> (indices_->size ());
276  std::vector<int> neighbours;
277  std::vector<float> distances;
278 
279  point_neighbours_.resize (input_->size (), neighbours);
280  point_distances_.resize (input_->size (), distances);
281 
282  for (int i_point = 0; i_point < point_number; i_point++)
283  {
284  int point_index = (*indices_)[i_point];
285  neighbours.clear ();
286  distances.clear ();
287  search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances);
288  point_neighbours_[point_index].swap (neighbours);
289  point_distances_[point_index].swap (distances);
290  }
291 }
292 
293 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
294 template <typename PointT, typename NormalT> void
296 {
297  std::vector<int> neighbours;
298  std::vector<float> distances;
299  segment_neighbours_.resize (number_of_segments_, neighbours);
300  segment_distances_.resize (number_of_segments_, distances);
301 
302  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
303  {
304  std::vector<int> nghbrs;
305  std::vector<float> dist;
306  findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
307  segment_neighbours_[i_seg].swap (nghbrs);
308  segment_distances_[i_seg].swap (dist);
309  }
310 }
311 
312 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
313 template <typename PointT,typename NormalT> void
314 pcl::RegionGrowingRGB<PointT, NormalT>::findRegionsKNN (int index, int nghbr_number, std::vector<int>& nghbrs, std::vector<float>& dist)
315 {
316  std::vector<float> distances;
317  float max_dist = std::numeric_limits<float>::max ();
318  distances.resize (clusters_.size (), max_dist);
319 
320  int number_of_points = num_pts_in_segment_[index];
321  //loop through every point in this segment and check neighbours
322  for (int i_point = 0; i_point < number_of_points; i_point++)
323  {
324  int point_index = clusters_[index].indices[i_point];
325  int number_of_neighbours = static_cast<int> (point_neighbours_[point_index].size ());
326  //loop through every neighbour of the current point, find out to which segment it belongs
327  //and if it belongs to neighbouring segment and is close enough then remember segment and its distance
328  for (int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
329  {
330  // find segment
331  int segment_index = -1;
332  segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
333 
334  if ( segment_index != index )
335  {
336  // try to push it to the queue
337  if (distances[segment_index] > point_distances_[point_index][i_nghbr])
338  distances[segment_index] = point_distances_[point_index][i_nghbr];
339  }
340  }
341  }// next point
342 
343  std::priority_queue<std::pair<float, int> > segment_neighbours;
344  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
345  {
346  if (distances[i_seg] < max_dist)
347  {
348  segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) );
349  if (int (segment_neighbours.size ()) > nghbr_number)
350  segment_neighbours.pop ();
351  }
352  }
353 
354  int size = std::min<int> (static_cast<int> (segment_neighbours.size ()), nghbr_number);
355  nghbrs.resize (size, 0);
356  dist.resize (size, 0);
357  int counter = 0;
358  while ( !segment_neighbours.empty () && counter < nghbr_number )
359  {
360  dist[counter] = segment_neighbours.top ().first;
361  nghbrs[counter] = segment_neighbours.top ().second;
362  segment_neighbours.pop ();
363  counter++;
364  }
365 }
366 
367 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
368 template <typename PointT, typename NormalT> void
370 {
371  // calculate color of each segment
372  std::vector< std::vector<unsigned int> > segment_color;
373  std::vector<unsigned int> color;
374  color.resize (3, 0);
375  segment_color.resize (number_of_segments_, color);
376 
377  for (const auto& point_index : (*indices_))
378  {
379  int segment_index = point_labels_[point_index];
380  segment_color[segment_index][0] += (*input_)[point_index].r;
381  segment_color[segment_index][1] += (*input_)[point_index].g;
382  segment_color[segment_index][2] += (*input_)[point_index].b;
383  }
384  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
385  {
386  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]));
387  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]));
388  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]));
389  }
390 
391  // now it is time to find out if there are segments with a similar color
392  // and merge them together
393  std::vector<unsigned int> num_pts_in_homogeneous_region;
394  std::vector<int> num_seg_in_homogeneous_region;
395 
396  segment_labels_.resize (number_of_segments_, -1);
397 
398  float dist_thresh = distance_threshold_;
399  int homogeneous_region_number = 0;
400  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
401  {
402  int curr_homogeneous_region = 0;
403  if (segment_labels_[i_seg] == -1)
404  {
405  segment_labels_[i_seg] = homogeneous_region_number;
406  curr_homogeneous_region = homogeneous_region_number;
407  num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
408  num_seg_in_homogeneous_region.push_back (1);
409  homogeneous_region_number++;
410  }
411  else
412  curr_homogeneous_region = segment_labels_[i_seg];
413 
414  unsigned int i_nghbr = 0;
415  while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
416  {
417  int index = segment_neighbours_[i_seg][i_nghbr];
418  if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
419  {
420  i_nghbr++;
421  continue;
422  }
423  if ( segment_labels_[index] == -1 )
424  {
425  float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
426  if (difference < color_r2r_threshold_)
427  {
428  segment_labels_[index] = curr_homogeneous_region;
429  num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
430  num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
431  }
432  }
433  i_nghbr++;
434  }// next neighbour
435  }// next segment
436 
437  segment_color.clear ();
438  color.clear ();
439 
440  std::vector< std::vector<int> > final_segments;
441  std::vector<int> region;
442  final_segments.resize (homogeneous_region_number, region);
443  for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
444  {
445  final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
446  }
447 
448  std::vector<int> counter;
449  counter.resize (homogeneous_region_number, 0);
450  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
451  {
452  int index = segment_labels_[i_seg];
453  final_segments[ index ][ counter[index] ] = i_seg;
454  counter[index] += 1;
455  }
456 
457  std::vector< std::vector< std::pair<float, int> > > region_neighbours;
458  findRegionNeighbours (region_neighbours, final_segments);
459 
460  int final_segment_number = homogeneous_region_number;
461  for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
462  {
463  if (static_cast<int> (num_pts_in_homogeneous_region[i_reg]) < min_pts_per_cluster_)
464  {
465  if ( region_neighbours[i_reg].empty () )
466  continue;
467  int nearest_neighbour = region_neighbours[i_reg][0].second;
468  if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
469  continue;
470  int reg_index = segment_labels_[nearest_neighbour];
471  int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
472  for (int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
473  {
474  int segment_index = final_segments[i_reg][i_seg];
475  final_segments[reg_index].push_back (segment_index);
476  segment_labels_[segment_index] = reg_index;
477  }
478  final_segments[i_reg].clear ();
479  num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
480  num_pts_in_homogeneous_region[i_reg] = 0;
481  num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
482  num_seg_in_homogeneous_region[i_reg] = 0;
483  final_segment_number -= 1;
484 
485  int nghbr_number = static_cast<int> (region_neighbours[reg_index].size ());
486  for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
487  {
488  if ( segment_labels_[ region_neighbours[reg_index][i_nghbr].second ] == reg_index )
489  {
490  region_neighbours[reg_index][i_nghbr].first = std::numeric_limits<float>::max ();
491  region_neighbours[reg_index][i_nghbr].second = 0;
492  }
493  }
494  nghbr_number = static_cast<int> (region_neighbours[i_reg].size ());
495  for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
496  {
497  if ( segment_labels_[ region_neighbours[i_reg][i_nghbr].second ] != reg_index )
498  {
499  std::pair<float, int> pair;
500  pair.first = region_neighbours[i_reg][i_nghbr].first;
501  pair.second = region_neighbours[i_reg][i_nghbr].second;
502  region_neighbours[reg_index].push_back (pair);
503  }
504  }
505  region_neighbours[i_reg].clear ();
506  std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (), comparePair);
507  }
508  }
509 
510  assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ()));
511 
512  number_of_segments_ = final_segment_number;
513 }
514 
515 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
516 template <typename PointT, typename NormalT> float
517 pcl::RegionGrowingRGB<PointT, NormalT>::calculateColorimetricalDifference (std::vector<unsigned int>& first_color, std::vector<unsigned int>& second_color) const
518 {
519  float difference = 0.0f;
520  difference += float ((first_color[0] - second_color[0]) * (first_color[0] - second_color[0]));
521  difference += float ((first_color[1] - second_color[1]) * (first_color[1] - second_color[1]));
522  difference += float ((first_color[2] - second_color[2]) * (first_color[2] - second_color[2]));
523  return (difference);
524 }
525 
526 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
527 template <typename PointT, typename NormalT> void
528 pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::vector< std::pair<float, int> > >& neighbours_out, std::vector< std::vector<int> >& regions_in)
529 {
530  int region_number = static_cast<int> (regions_in.size ());
531  neighbours_out.clear ();
532  neighbours_out.resize (region_number);
533 
534  for (int i_reg = 0; i_reg < region_number; i_reg++)
535  {
536  int segment_num = static_cast<int> (regions_in[i_reg].size ());
537  neighbours_out[i_reg].reserve (segment_num * region_neighbour_number_);
538  for (int i_seg = 0; i_seg < segment_num; i_seg++)
539  {
540  int curr_segment = regions_in[i_reg][i_seg];
541  int nghbr_number = static_cast<int> (segment_neighbours_[curr_segment].size ());
542  std::pair<float, int> pair;
543  for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
544  {
545  int segment_index = segment_neighbours_[curr_segment][i_nghbr];
546  if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
547  continue;
548  if (segment_labels_[segment_index] != i_reg)
549  {
550  pair.first = segment_distances_[curr_segment][i_nghbr];
551  pair.second = segment_index;
552  neighbours_out[i_reg].push_back (pair);
553  }
554  }// next neighbour
555  }// next segment
556  std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (), comparePair);
557  }// next homogeneous region
558 }
559 
560 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
561 template <typename PointT, typename NormalT> void
562 pcl::RegionGrowingRGB<PointT, NormalT>::assembleRegions (std::vector<unsigned int>& num_pts_in_region, int num_regions)
563 {
564  clusters_.clear ();
565  pcl::PointIndices segment;
566  clusters_.resize (num_regions, segment);
567  for (int i_seg = 0; i_seg < num_regions; i_seg++)
568  {
569  clusters_[i_seg].indices.resize (num_pts_in_region[i_seg]);
570  }
571 
572  std::vector<int> counter;
573  counter.resize (num_regions, 0);
574  int point_number = static_cast<int> (indices_->size ());
575  for (int i_point = 0; i_point < point_number; i_point++)
576  {
577  int point_index = (*indices_)[i_point];
578  int index = point_labels_[point_index];
579  index = segment_labels_[index];
580  clusters_[index].indices[ counter[index] ] = point_index;
581  counter[index] += 1;
582  }
583 
584  // now we need to erase empty regions
585  if (clusters_.empty ())
586  return;
587 
588  std::vector<pcl::PointIndices>::iterator itr1, itr2;
589  itr1 = clusters_.begin ();
590  itr2 = clusters_.end () - 1;
591 
592  while (itr1 < itr2)
593  {
594  while (!(itr1->indices.empty ()) && itr1 < itr2)
595  ++itr1;
596  while ( itr2->indices.empty () && itr1 < itr2)
597  --itr2;
598 
599  if (itr1 != itr2)
600  itr1->indices.swap (itr2->indices);
601  }
602 
603  if (itr2->indices.empty ())
604  clusters_.erase (itr2, clusters_.end ());
605 }
606 
607 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
608 template <typename PointT, typename NormalT> bool
609 pcl::RegionGrowingRGB<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const
610 {
611  is_a_seed = true;
612 
613  // check the color difference
614  std::vector<unsigned int> point_color;
615  point_color.resize (3, 0);
616  std::vector<unsigned int> nghbr_color;
617  nghbr_color.resize (3, 0);
618  point_color[0] = (*input_)[point].r;
619  point_color[1] = (*input_)[point].g;
620  point_color[2] = (*input_)[point].b;
621  nghbr_color[0] = (*input_)[nghbr].r;
622  nghbr_color[1] = (*input_)[nghbr].g;
623  nghbr_color[2] = (*input_)[nghbr].b;
624  float difference = calculateColorimetricalDifference (point_color, nghbr_color);
625  if (difference > color_p2p_threshold_)
626  return (false);
627 
628  float cosine_threshold = std::cos (theta_threshold_);
629 
630  // check the angle between normals if needed
631  if (normal_flag_)
632  {
633  float data[4];
634  data[0] = (*input_)[point].data[0];
635  data[1] = (*input_)[point].data[1];
636  data[2] = (*input_)[point].data[2];
637  data[3] = (*input_)[point].data[3];
638 
639  Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
640  Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
641  if (smooth_mode_flag_ == true)
642  {
643  Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
644  float dot_product = std::abs (nghbr_normal.dot (initial_normal));
645  if (dot_product < cosine_threshold)
646  return (false);
647  }
648  else
649  {
650  Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
651  Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> ((*normals_)[initial_seed].normal));
652  float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
653  if (dot_product < cosine_threshold)
654  return (false);
655  }
656  }
657 
658  // check the curvature if needed
659  if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
660  is_a_seed = false;
661 
662  // check the residual if needed
663  if (residual_flag_)
664  {
665  float data_p[4];
666  data_p[0] = (*input_)[point].data[0];
667  data_p[1] = (*input_)[point].data[1];
668  data_p[2] = (*input_)[point].data[2];
669  data_p[3] = (*input_)[point].data[3];
670  float data_n[4];
671  data_n[0] = (*input_)[nghbr].data[0];
672  data_n[1] = (*input_)[nghbr].data[1];
673  data_n[2] = (*input_)[nghbr].data[2];
674  data_n[3] = (*input_)[nghbr].data[3];
675  Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
676  Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
677  Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
678  float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
679  if (residual > residual_threshold_)
680  is_a_seed = false;
681  }
682 
683  return (true);
684 }
685 
686 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
687 template <typename PointT, typename NormalT> void
689 {
690  cluster.indices.clear ();
691 
692  bool segmentation_is_possible = initCompute ();
693  if ( !segmentation_is_possible )
694  {
695  deinitCompute ();
696  return;
697  }
698 
699  // first of all we need to find out if this point belongs to cloud
700  bool point_was_found = false;
701  for (const auto& point : (*indices_))
702  if (point == index)
703  {
704  point_was_found = true;
705  break;
706  }
707 
708  if (point_was_found)
709  {
710  if (clusters_.empty ())
711  {
712  clusters_.clear ();
713  point_neighbours_.clear ();
714  point_labels_.clear ();
715  num_pts_in_segment_.clear ();
716  point_distances_.clear ();
717  segment_neighbours_.clear ();
718  segment_distances_.clear ();
719  segment_labels_.clear ();
720  number_of_segments_ = 0;
721 
722  segmentation_is_possible = prepareForSegmentation ();
723  if ( !segmentation_is_possible )
724  {
725  deinitCompute ();
726  return;
727  }
728 
729  findPointNeighbours ();
730  applySmoothRegionGrowingAlgorithm ();
732 
733  findSegmentNeighbours ();
734  applyRegionMergingAlgorithm ();
735  }
736  // if we have already made the segmentation, then find the segment
737  // to which this point belongs
738  for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
739  {
740  bool segment_was_found = false;
741  for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
742  {
743  if (i_segment->indices[i_point] == index)
744  {
745  segment_was_found = true;
746  cluster.indices.clear ();
747  cluster.indices.reserve (i_segment->indices.size ());
748  std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices));
749  break;
750  }
751  }
752  if (segment_was_found)
753  {
754  break;
755  }
756  }// next segment
757  }// end if point was found
758 
759  deinitCompute ();
760 }
761 
762 #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:94
pcl::RegionGrowingRGB::setNormalTestFlag
void setNormalTestFlag(bool value)
Allows to turn on/off the smoothness test.
Definition: region_growing_rgb.hpp:143
pcl::RegionGrowingRGB::getNormalTestFlag
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
Definition: region_growing_rgb.hpp:136
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:323
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:273
pcl::RegionGrowingRGB::getNumberOfRegionNeighbours
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
Definition: region_growing_rgb.hpp:122
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:291
pcl::RegionGrowingRGB::setCurvatureTestFlag
void setCurvatureTestFlag(bool value) override
Allows to turn on/off the curvature test.
Definition: region_growing_rgb.hpp:150
pcl::RegionGrowingRGB::getSegmentFromPoint
void getSegmentFromPoint(index_t 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:688
pcl::RegionGrowingRGB::setPointColorThreshold
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.
Definition: region_growing_rgb.hpp:87
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
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:164
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:517
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:294
pcl::RegionGrowingRGB::setResidualTestFlag
void setResidualTestFlag(bool value) override
Allows to turn on/off the residual test.
Definition: region_growing_rgb.hpp:157
pcl::RegionGrowingRGB::~RegionGrowingRGB
~RegionGrowingRGB()
Destructor that frees memory.
Definition: region_growing_rgb.hpp:70
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:218
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:340
pcl::RegionGrowingRGB::findRegionsKNN
void findRegionsKNN(index_t index, int nghbr_number, Indices &nghbrs, std::vector< float > &dist)
This method finds K nearest neighbours of the given segment.
Definition: region_growing_rgb.hpp:314
pcl::search::KdTree< PointT >
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:295
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:129
pcl::RegionGrowingRGB::setDistanceThreshold
void setDistanceThreshold(float thresh)
Allows to set distance threshold.
Definition: region_growing_rgb.hpp:115
pcl::PointIndices
Definition: PointIndices.h:11
pcl::RegionGrowingRGB::setRegionColorThreshold
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
Definition: region_growing_rgb.hpp:101
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:282
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:80
pcl::RegionGrowingRGB::validatePoint
bool validatePoint(index_t initial_seed, index_t point, index_t 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:609
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:528
pcl::RegionGrowingRGB::RegionGrowingRGB
RegionGrowingRGB()
Constructor that sets default values for member variables.
Definition: region_growing_rgb.hpp:52
pcl::RegionGrowingRGB::applyRegionMergingAlgorithm
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
Definition: region_growing_rgb.hpp:369
pcl::RegionGrowingRGB::getDistanceThreshold
float getDistanceThreshold() const
Returns the distance threshold.
Definition: region_growing_rgb.hpp:108