Point Cloud Library (PCL)  1.8.1
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.size () == 0 )
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_ == 0 || 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 throug 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 throug 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 = cosf (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 = fabsf (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 = fabsf (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 = fabsf (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  std::vector <pcl::PointIndices>::iterator i_segment;
743  for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
744  {
745  bool segment_was_found = false;
746  for (size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
747  {
748  if (i_segment->indices[i_point] == index)
749  {
750  segment_was_found = true;
751  cluster.indices.clear ();
752  cluster.indices.reserve (i_segment->indices.size ());
753  std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices));
754  break;
755  }
756  }
757  if (segment_was_found)
758  {
759  break;
760  }
761  }// next segment
762  }// end if point was found
763 
764  deinitCompute ();
765 }
766 
767 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
RegionGrowingRGB()
Constructor that sets default values for member variables.
virtual ~RegionGrowingRGB()
Destructor that frees memory.
bool residual_flag_
If set to true then residual test will be done during segmentation.
std::vector< int > indices
Definition: PointIndices.h:19
void setNumberOfRegionNeighbours(unsigned int nghbr_number)
This method allows to set the number of neighbours that is used for finding neighbouring segments...
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.
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.
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use...
virtual bool validatePoint(int initial_seed, int point, int nghbr, bool &is_a_seed) const
This function is checking if the point with index &#39;nghbr&#39; belongs to the segment. ...
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.
float getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
float getDistanceThreshold() const
Returns the distance threshold.
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
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 > > &regions_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.
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...
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.