Point Cloud Library (PCL)  1.10.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/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 ();
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 
193 
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
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_)
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  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
403  {
404  int curr_homogeneous_region = 0;
405  if (segment_labels_[i_seg] == -1)
406  {
407  segment_labels_[i_seg] = homogeneous_region_number;
408  curr_homogeneous_region = homogeneous_region_number;
409  num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
410  num_seg_in_homogeneous_region.push_back (1);
411  homogeneous_region_number++;
412  }
413  else
414  curr_homogeneous_region = segment_labels_[i_seg];
415 
416  unsigned int i_nghbr = 0;
417  while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
418  {
419  int index = segment_neighbours_[i_seg][i_nghbr];
420  if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
421  {
422  i_nghbr++;
423  continue;
424  }
425  if ( segment_labels_[index] == -1 )
426  {
427  float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
428  if (difference < color_r2r_threshold_)
429  {
430  segment_labels_[index] = curr_homogeneous_region;
431  num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
432  num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
433  }
434  }
435  i_nghbr++;
436  }// next neighbour
437  }// next segment
438 
439  segment_color.clear ();
440  color.clear ();
441 
442  std::vector< std::vector<int> > final_segments;
443  std::vector<int> region;
444  final_segments.resize (homogeneous_region_number, region);
445  for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
446  {
447  final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
448  }
449 
450  std::vector<int> counter;
451  counter.resize (homogeneous_region_number, 0);
452  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
453  {
454  int index = segment_labels_[i_seg];
455  final_segments[ index ][ counter[index] ] = i_seg;
456  counter[index] += 1;
457  }
458 
459  std::vector< std::vector< std::pair<float, int> > > region_neighbours;
460  findRegionNeighbours (region_neighbours, final_segments);
461 
462  int final_segment_number = homogeneous_region_number;
463  for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
464  {
465  if (static_cast<int> (num_pts_in_homogeneous_region[i_reg]) < min_pts_per_cluster_)
466  {
467  if ( region_neighbours[i_reg].empty () )
468  continue;
469  int nearest_neighbour = region_neighbours[i_reg][0].second;
470  if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
471  continue;
472  int reg_index = segment_labels_[nearest_neighbour];
473  int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
474  for (int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
475  {
476  int segment_index = final_segments[i_reg][i_seg];
477  final_segments[reg_index].push_back (segment_index);
478  segment_labels_[segment_index] = reg_index;
479  }
480  final_segments[i_reg].clear ();
481  num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
482  num_pts_in_homogeneous_region[i_reg] = 0;
483  num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
484  num_seg_in_homogeneous_region[i_reg] = 0;
485  final_segment_number -= 1;
486 
487  int nghbr_number = static_cast<int> (region_neighbours[reg_index].size ());
488  for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
489  {
490  if ( segment_labels_[ region_neighbours[reg_index][i_nghbr].second ] == reg_index )
491  {
492  region_neighbours[reg_index][i_nghbr].first = std::numeric_limits<float>::max ();
493  region_neighbours[reg_index][i_nghbr].second = 0;
494  }
495  }
496  nghbr_number = static_cast<int> (region_neighbours[i_reg].size ());
497  for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
498  {
499  if ( segment_labels_[ region_neighbours[i_reg][i_nghbr].second ] != reg_index )
500  {
501  std::pair<float, int> pair;
502  pair.first = region_neighbours[i_reg][i_nghbr].first;
503  pair.second = region_neighbours[i_reg][i_nghbr].second;
504  region_neighbours[reg_index].push_back (pair);
505  }
506  }
507  region_neighbours[i_reg].clear ();
508  std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (), comparePair);
509  }
510  }
511 
512  assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ()));
513 
514  number_of_segments_ = final_segment_number;
515 }
516 
517 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
518 template <typename PointT, typename NormalT> float
519 pcl::RegionGrowingRGB<PointT, NormalT>::calculateColorimetricalDifference (std::vector<unsigned int>& first_color, std::vector<unsigned int>& second_color) const
520 {
521  float difference = 0.0f;
522  difference += float ((first_color[0] - second_color[0]) * (first_color[0] - second_color[0]));
523  difference += float ((first_color[1] - second_color[1]) * (first_color[1] - second_color[1]));
524  difference += float ((first_color[2] - second_color[2]) * (first_color[2] - second_color[2]));
525  return (difference);
526 }
527 
528 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
529 template <typename PointT, typename NormalT> void
530 pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::vector< std::pair<float, int> > >& neighbours_out, std::vector< std::vector<int> >& regions_in)
531 {
532  int region_number = static_cast<int> (regions_in.size ());
533  neighbours_out.clear ();
534  neighbours_out.resize (region_number);
535 
536  for (int i_reg = 0; i_reg < region_number; i_reg++)
537  {
538  int segment_num = static_cast<int> (regions_in[i_reg].size ());
539  neighbours_out[i_reg].reserve (segment_num * region_neighbour_number_);
540  for (int i_seg = 0; i_seg < segment_num; i_seg++)
541  {
542  int curr_segment = regions_in[i_reg][i_seg];
543  int nghbr_number = static_cast<int> (segment_neighbours_[curr_segment].size ());
544  std::pair<float, int> pair;
545  for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
546  {
547  int segment_index = segment_neighbours_[curr_segment][i_nghbr];
548  if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
549  continue;
550  if (segment_labels_[segment_index] != i_reg)
551  {
552  pair.first = segment_distances_[curr_segment][i_nghbr];
553  pair.second = segment_index;
554  neighbours_out[i_reg].push_back (pair);
555  }
556  }// next neighbour
557  }// next segment
558  std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (), comparePair);
559  }// next homogeneous region
560 }
561 
562 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
563 template <typename PointT, typename NormalT> void
564 pcl::RegionGrowingRGB<PointT, NormalT>::assembleRegions (std::vector<unsigned int>& num_pts_in_region, int num_regions)
565 {
566  clusters_.clear ();
567  pcl::PointIndices segment;
568  clusters_.resize (num_regions, segment);
569  for (int i_seg = 0; i_seg < num_regions; i_seg++)
570  {
571  clusters_[i_seg].indices.resize (num_pts_in_region[i_seg]);
572  }
573 
574  std::vector<int> counter;
575  counter.resize (num_regions, 0);
576  int point_number = static_cast<int> (indices_->size ());
577  for (int i_point = 0; i_point < point_number; i_point++)
578  {
579  int point_index = (*indices_)[i_point];
580  int index = point_labels_[point_index];
581  index = segment_labels_[index];
582  clusters_[index].indices[ counter[index] ] = point_index;
583  counter[index] += 1;
584  }
585 
586  // now we need to erase empty regions
587  if (clusters_.empty ())
588  return;
589 
590  std::vector<pcl::PointIndices>::iterator itr1, itr2;
591  itr1 = clusters_.begin ();
592  itr2 = clusters_.end () - 1;
593 
594  while (itr1 < itr2)
595  {
596  while (!(itr1->indices.empty ()) && itr1 < itr2)
597  ++itr1;
598  while ( itr2->indices.empty () && itr1 < itr2)
599  --itr2;
600 
601  if (itr1 != itr2)
602  itr1->indices.swap (itr2->indices);
603  }
604 
605  if (itr2->indices.empty ())
606  clusters_.erase (itr2, clusters_.end ());
607 }
608 
609 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
610 template <typename PointT, typename NormalT> bool
611 pcl::RegionGrowingRGB<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const
612 {
613  is_a_seed = true;
614 
615  // check the color difference
616  std::vector<unsigned int> point_color;
617  point_color.resize (3, 0);
618  std::vector<unsigned int> nghbr_color;
619  nghbr_color.resize (3, 0);
620  point_color[0] = input_->points[point].r;
621  point_color[1] = input_->points[point].g;
622  point_color[2] = input_->points[point].b;
623  nghbr_color[0] = input_->points[nghbr].r;
624  nghbr_color[1] = input_->points[nghbr].g;
625  nghbr_color[2] = input_->points[nghbr].b;
626  float difference = calculateColorimetricalDifference (point_color, nghbr_color);
627  if (difference > color_p2p_threshold_)
628  return (false);
629 
630  float cosine_threshold = std::cos (theta_threshold_);
631 
632  // check the angle between normals if needed
633  if (normal_flag_)
634  {
635  float data[4];
636  data[0] = input_->points[point].data[0];
637  data[1] = input_->points[point].data[1];
638  data[2] = input_->points[point].data[2];
639  data[3] = input_->points[point].data[3];
640 
641  Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
642  Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
643  if (smooth_mode_flag_ == true)
644  {
645  Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
646  float dot_product = std::abs (nghbr_normal.dot (initial_normal));
647  if (dot_product < cosine_threshold)
648  return (false);
649  }
650  else
651  {
652  Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
653  Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal));
654  float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
655  if (dot_product < cosine_threshold)
656  return (false);
657  }
658  }
659 
660  // check the curvature if needed
661  if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
662  is_a_seed = false;
663 
664  // check the residual if needed
665  if (residual_flag_)
666  {
667  float data_p[4];
668  data_p[0] = input_->points[point].data[0];
669  data_p[1] = input_->points[point].data[1];
670  data_p[2] = input_->points[point].data[2];
671  data_p[3] = input_->points[point].data[3];
672  float data_n[4];
673  data_n[0] = input_->points[nghbr].data[0];
674  data_n[1] = input_->points[nghbr].data[1];
675  data_n[2] = input_->points[nghbr].data[2];
676  data_n[3] = input_->points[nghbr].data[3];
677  Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
678  Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
679  Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
680  float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
681  if (residual > residual_threshold_)
682  is_a_seed = false;
683  }
684 
685  return (true);
686 }
687 
688 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
689 template <typename PointT, typename NormalT> void
691 {
692  cluster.indices.clear ();
693 
694  bool segmentation_is_possible = initCompute ();
695  if ( !segmentation_is_possible )
696  {
697  deinitCompute ();
698  return;
699  }
700 
701  // first of all we need to find out if this point belongs to cloud
702  bool point_was_found = false;
703  int number_of_points = static_cast <int> (indices_->size ());
704  for (int point = 0; point < number_of_points; point++)
705  if ( (*indices_)[point] == index)
706  {
707  point_was_found = true;
708  break;
709  }
710 
711  if (point_was_found)
712  {
713  if (clusters_.empty ())
714  {
715  clusters_.clear ();
716  point_neighbours_.clear ();
717  point_labels_.clear ();
718  num_pts_in_segment_.clear ();
719  point_distances_.clear ();
720  segment_neighbours_.clear ();
721  segment_distances_.clear ();
722  segment_labels_.clear ();
724 
725  segmentation_is_possible = prepareForSegmentation ();
726  if ( !segmentation_is_possible )
727  {
728  deinitCompute ();
729  return;
730  }
731 
735 
738  }
739  // if we have already made the segmentation, then find the segment
740  // to which this point belongs
741  for (auto i_segment = clusters_.cbegin (); i_segment != clusters_.cend (); i_segment++)
742  {
743  bool segment_was_found = false;
744  for (std::size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
745  {
746  if (i_segment->indices[i_point] == index)
747  {
748  segment_was_found = true;
749  cluster.indices.clear ();
750  cluster.indices.reserve (i_segment->indices.size ());
751  std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices));
752  break;
753  }
754  }
755  if (segment_was_found)
756  {
757  break;
758  }
759  }// next segment
760  }// end if point was found
761 
762  deinitCompute ();
763 }
764 
765 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
float theta_threshold_
Thershold used for testing the smoothness between points.
float distance_threshold_
Threshold that tells which points we need to assume neighbouring.
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
float residual_threshold_
Thershold used in residual test.
float curvature_threshold_
Thershold used in curvature test.
RegionGrowingRGB()
Constructor that sets default values for member variables.
~RegionGrowingRGB()
Destructor that frees memory.
std::vector< std::vector< float > > segment_distances_
Stores distances for the segment neighbours from segment_neighbours_.
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...
unsigned int region_neighbour_number_
Number of neighbouring segments to find.
std::vector< int > num_pts_in_segment_
Tells how much points each segment contains.
void setResidualTestFlag(bool value) override
Allows to turn on/off the residual test.
unsigned int neighbour_number_
Number of neighbours to find.
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:151
bool residual_flag_
If set to true then residual test will be done during segmentation.
void setNumberOfRegionNeighbours(unsigned int nghbr_number)
This method allows to set the number of neighbours that is used for finding neighbouring segments...
std::vector< std::vector< float > > point_distances_
Stores distances for the point neighbours from point_neighbours_.
int min_pts_per_cluster_
Stores the minimum number of points that a cluster needs to contain in order to be considered valid...
int number_of_segments_
Stores the number of segments.
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138
NormalPtr normals_
Contains normals of the points that will be segmented.
void findRegionsKNN(int index, int nghbr_number, std::vector< int > &nghbrs, std::vector< float > &dist)
This method finds K nearest neighbours of the given segment.
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
int max_pts_per_cluster_
Stores the maximum number of points that a cluster needs to contain in order to be considered valid...
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use...
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
bool validatePoint(int initial_seed, int point, int nghbr, bool &is_a_seed) const override
This function is checking if the point with index &#39;nghbr&#39; belongs to the segment. ...
std::vector< std::vector< int > > segment_neighbours_
Stores the neighboures for the corresponding segments.
float calculateColorimetricalDifference(std::vector< unsigned int > &first_color, std::vector< unsigned int > &second_color) const
This method calculates the colorimetrical difference between two points.
std::vector< pcl::PointIndices > clusters_
After the segmentation it will contain the segments.
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.
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:171
std::vector< int > point_labels_
Point labels that tells to which segment each point belongs.
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
bool prepareForSegmentation() override
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
std::vector< int > segment_labels_
Stores new indices for segments that were obtained at the region growing stage.
void applySmoothRegionGrowingAlgorithm()
This function implements the algorithm described in the article "Segmentation of point clouds using s...
bool smooth_mode_flag_
Flag that signalizes if the smoothness constraint will be used.
float color_r2r_threshold_
Thershold used in color test for regions.
void setDistanceThreshold(float thresh)
Allows to set distance threshold.
void findRegionNeighbours(std::vector< std::vector< std::pair< float, int > > > &neighbours_out, std::vector< std::vector< int > > &regions_in)
This method assembles the array containing neighbours of each homogeneous region. ...
void assembleRegions()
This function simply assembles the regions from list of point labels.
void extract(std::vector< pcl::PointIndices > &clusters) override
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
bool normal_flag_
If set to true then normal/smoothness test will be done during segmentation.
KdTreePtr search_
Serch method that will be used for KNN.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:148
void setNormalTestFlag(bool value)
Allows to turn on/off the smoothness test.
void setCurvatureTestFlag(bool value) override
Allows to turn on/off the curvature test.
float color_p2p_threshold_
Thershold used in color test for points.
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
void findPointNeighbours() override
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
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.
std::vector< std::vector< int > > point_neighbours_
Contains neighbours of each point.