Point Cloud Library (PCL)  1.7.0
region_growing_rgb.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the copyright holder(s) nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author : Sergey Ushakov
36  * Email : mine_all_mine@bk.ru
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
41 #define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
42 
43 #include <pcl/segmentation/region_growing_rgb.h>
44 #include <pcl/search/search.h>
45 #include <pcl/search/kdtree.h>
46 
47 #include <queue>
48 
49 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointT, typename NormalT>
52  color_p2p_threshold_ (1225.0f),
53  color_r2r_threshold_ (10.0f),
54  distance_threshold_ (0.05f),
55  region_neighbour_number_ (100),
56  point_distances_ (0),
57  segment_neighbours_ (0),
58  segment_distances_ (0),
59  segment_labels_ (0)
60 {
61  normal_flag_ = false;
62  curvature_flag_ = false;
63  residual_flag_ = false;
65 }
66 
67 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointT, typename NormalT>
70 {
71  point_distances_.clear ();
72  segment_neighbours_.clear ();
73  segment_distances_.clear ();
74  segment_labels_.clear ();
75 }
76 
77 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78 template <typename PointT, typename NormalT> float
80 {
81  return (powf (color_p2p_threshold_, 0.5f));
82 }
83 
84 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
85 template <typename PointT, typename NormalT> void
87 {
88  color_p2p_threshold_ = thresh * thresh;
89 }
90 
91 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
92 template <typename PointT, typename NormalT> float
94 {
95  return (powf (color_r2r_threshold_, 0.5f));
96 }
97 
98 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
99 template <typename PointT, typename NormalT> void
101 {
102  color_r2r_threshold_ = thresh * thresh;
103 }
104 
105 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
106 template <typename PointT, typename NormalT> float
108 {
109  return (powf (distance_threshold_, 0.5f));
110 }
111 
112 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
113 template <typename PointT, typename NormalT> void
115 {
116  distance_threshold_ = thresh * thresh;
117 }
118 
119 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
120 template <typename PointT, typename NormalT> unsigned int
122 {
123  return (region_neighbour_number_);
124 }
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
127 template <typename PointT, typename NormalT> void
129 {
130  region_neighbour_number_ = nghbr_number;
131 }
132 
133 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
134 template <typename PointT, typename NormalT> bool
136 {
137  return (normal_flag_);
138 }
139 
140 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
141 template <typename PointT, typename NormalT> void
143 {
144  normal_flag_ = value;
145 }
146 
147 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
148 template <typename PointT, typename NormalT> void
150 {
151  curvature_flag_ = value;
152 }
153 
154 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
155 template <typename PointT, typename NormalT> void
157 {
158  residual_flag_ = value;
159 }
160 
161 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
162 template <typename PointT, typename NormalT> void
163 pcl::RegionGrowingRGB<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
164 {
165  clusters_.clear ();
166  clusters.clear ();
167  point_neighbours_.clear ();
168  point_labels_.clear ();
169  num_pts_in_segment_.clear ();
170  point_distances_.clear ();
171  segment_neighbours_.clear ();
172  segment_distances_.clear ();
173  segment_labels_.clear ();
174  number_of_segments_ = 0;
175 
176  bool segmentation_is_possible = initCompute ();
177  if ( !segmentation_is_possible )
178  {
179  deinitCompute ();
180  return;
181  }
182 
183  segmentation_is_possible = prepareForSegmentation ();
184  if ( !segmentation_is_possible )
185  {
186  deinitCompute ();
187  return;
188  }
189 
190  findPointNeighbours ();
191  applySmoothRegionGrowingAlgorithm ();
193 
194  findSegmentNeighbours ();
195  applyRegionMergingAlgorithm ();
196 
197  std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin ();
198  while (cluster_iter != clusters_.end ())
199  {
200  if (cluster_iter->indices.size () < min_pts_per_cluster_ || cluster_iter->indices.size () > max_pts_per_cluster_)
201  {
202  cluster_iter = clusters_.erase (cluster_iter);
203  }
204  else
205  cluster_iter++;
206  }
207 
208  clusters.reserve (clusters_.size ());
209  std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
210 
211  deinitCompute ();
212 }
213 
214 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
215 template <typename PointT, typename NormalT> bool
217 {
218  // if user forgot to pass point cloud or if it is empty
219  if ( input_->points.size () == 0 )
220  return (false);
221 
222  // if normal/smoothness test is on then we need to check if all needed variables and parameters
223  // were correctly initialized
224  if (normal_flag_)
225  {
226  // if user forgot to pass normals or the sizes of point and normal cloud are different
227  if ( normals_ == 0 || input_->points.size () != normals_->points.size () )
228  return (false);
229  }
230 
231  // if residual test is on then we need to check if all needed parameters were correctly initialized
232  if (residual_flag_)
233  {
234  if (residual_threshold_ <= 0.0f)
235  return (false);
236  }
237 
238  // if curvature test is on ...
239  // if (curvature_flag_)
240  // {
241  // in this case we do not need to check anything that related to it
242  // so we simply commented it
243  // }
244 
245  // here we check the parameters related to color-based segmentation
246  if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f )
247  return (false);
248 
249  // from here we check those parameters that are always valuable
250  if (neighbour_number_ == 0)
251  return (false);
252 
253  // if user didn't set search method
254  if (!search_)
255  search_.reset (new pcl::search::KdTree<PointT>);
256 
257  if (indices_)
258  {
259  if (indices_->empty ())
260  PCL_ERROR ("[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
261  search_->setInputCloud (input_, indices_);
262  }
263  else
264  search_->setInputCloud (input_);
265 
266  return (true);
267 }
268 
269 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
270 template <typename PointT, typename NormalT> void
272 {
273  int point_number = static_cast<int> (indices_->size ());
274  std::vector<int> neighbours;
275  std::vector<float> distances;
276 
277  point_neighbours_.resize (input_->points.size (), neighbours);
278  point_distances_.resize (input_->points.size (), distances);
279 
280  for (int i_point = 0; i_point < point_number; i_point++)
281  {
282  int point_index = (*indices_)[i_point];
283  neighbours.clear ();
284  distances.clear ();
285  search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances);
286  point_neighbours_[point_index].swap (neighbours);
287  point_distances_[point_index].swap (distances);
288  }
289 }
290 
291 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
292 template <typename PointT, typename NormalT> void
294 {
295  std::vector<int> neighbours;
296  std::vector<float> distances;
297  segment_neighbours_.resize (number_of_segments_, neighbours);
298  segment_distances_.resize (number_of_segments_, distances);
299 
300  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
301  {
302  std::vector<int> nghbrs;
303  std::vector<float> dist;
304  findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
305  segment_neighbours_[i_seg].swap (nghbrs);
306  segment_distances_[i_seg].swap (dist);
307  }
308 }
309 
310 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
311 template <typename PointT,typename NormalT> void
312 pcl::RegionGrowingRGB<PointT, NormalT>::findRegionsKNN (int index, int nghbr_number, std::vector<int>& nghbrs, std::vector<float>& dist)
313 {
314  std::vector<float> distances;
315  float max_dist = std::numeric_limits<float>::max ();
316  distances.resize (clusters_.size (), max_dist);
317 
318  int number_of_points = num_pts_in_segment_[index];
319  //loop throug every point in this segment and check neighbours
320  for (int i_point = 0; i_point < number_of_points; i_point++)
321  {
322  int point_index = clusters_[index].indices[i_point];
323  int number_of_neighbours = static_cast<int> (point_neighbours_[point_index].size ());
324  //loop throug every neighbour of the current point, find out to which segment it belongs
325  //and if it belongs to neighbouring segment and is close enough then remember segment and its distance
326  for (int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
327  {
328  // find segment
329  int segment_index = -1;
330  segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
331 
332  if ( segment_index != index )
333  {
334  // try to push it to the queue
335  if (distances[segment_index] > point_distances_[point_index][i_nghbr])
336  distances[segment_index] = point_distances_[point_index][i_nghbr];
337  }
338  }
339  }// next point
340 
341  std::priority_queue<std::pair<float, int> > segment_neighbours;
342  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
343  {
344  if (distances[i_seg] < max_dist)
345  {
346  segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) );
347  if (int (segment_neighbours.size ()) > nghbr_number)
348  segment_neighbours.pop ();
349  }
350  }
351 
352  int size = std::min<int> (static_cast<int> (segment_neighbours.size ()), nghbr_number);
353  nghbrs.resize (size, 0);
354  dist.resize (size, 0);
355  int counter = 0;
356  while ( !segment_neighbours.empty () && counter < nghbr_number )
357  {
358  dist[counter] = segment_neighbours.top ().first;
359  nghbrs[counter] = segment_neighbours.top ().second;
360  segment_neighbours.pop ();
361  counter++;
362  }
363 }
364 
365 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
366 template <typename PointT, typename NormalT> void
368 {
369  int number_of_points = static_cast<int> (indices_->size ());
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 (int i_point = 0; i_point < number_of_points; i_point++)
378  {
379  int point_index = (*indices_)[i_point];
380  int segment_index = point_labels_[point_index];
381  segment_color[segment_index][0] += input_->points[point_index].r;
382  segment_color[segment_index][1] += input_->points[point_index].g;
383  segment_color[segment_index][2] += input_->points[point_index].b;
384  }
385  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
386  {
387  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]));
388  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]));
389  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]));
390  }
391 
392  // now it is time to find out if there are segments with a similar color
393  // and merge them together
394  std::vector<unsigned int> num_pts_in_homogeneous_region;
395  std::vector<int> num_seg_in_homogeneous_region;
396 
397  segment_labels_.resize (number_of_segments_, -1);
398 
399  float dist_thresh = distance_threshold_;
400  int homogeneous_region_number = 0;
401  int curr_homogeneous_region = 0;
402  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
403  {
404  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 (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  std::vector< pcl::PointIndices >::iterator i_region;
588  i_region = clusters_.begin ();
589  while(i_region != clusters_.end ())
590  {
591  if ( i_region->indices.empty () )
592  i_region = clusters_.erase (i_region);
593  else
594  i_region++;
595  }
596 }
597 
598 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
599 template <typename PointT, typename NormalT> bool
600 pcl::RegionGrowingRGB<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const
601 {
602  is_a_seed = true;
603 
604  // check the color difference
605  std::vector<unsigned int> point_color;
606  point_color.resize (3, 0);
607  std::vector<unsigned int> nghbr_color;
608  nghbr_color.resize (3, 0);
609  point_color[0] = input_->points[point].r;
610  point_color[1] = input_->points[point].g;
611  point_color[2] = input_->points[point].b;
612  nghbr_color[0] = input_->points[nghbr].r;
613  nghbr_color[1] = input_->points[nghbr].g;
614  nghbr_color[2] = input_->points[nghbr].b;
615  float difference = calculateColorimetricalDifference (point_color, nghbr_color);
616  if (difference > color_p2p_threshold_)
617  return (false);
618 
619  float cosine_threshold = cosf (theta_threshold_);
620 
621  // check the angle between normals if needed
622  if (normal_flag_)
623  {
624  float data[4];
625  data[0] = input_->points[point].data[0];
626  data[1] = input_->points[point].data[1];
627  data[2] = input_->points[point].data[2];
628  data[3] = input_->points[point].data[3];
629 
630  Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
631  Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
632  if (smooth_mode_flag_ == true)
633  {
634  Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
635  float dot_product = fabsf (nghbr_normal.dot (initial_normal));
636  if (dot_product < cosine_threshold)
637  return (false);
638  }
639  else
640  {
641  Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal));
642  Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal));
643  float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal));
644  if (dot_product < cosine_threshold)
645  return (false);
646  }
647  }
648 
649  // check the curvature if needed
650  if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_)
651  is_a_seed = false;
652 
653  // check the residual if needed
654  if (residual_flag_)
655  {
656  float data_p[4];
657  data_p[0] = input_->points[point].data[0];
658  data_p[1] = input_->points[point].data[1];
659  data_p[2] = input_->points[point].data[2];
660  data_p[3] = input_->points[point].data[3];
661  float data_n[4];
662  data_n[0] = input_->points[nghbr].data[0];
663  data_n[1] = input_->points[nghbr].data[1];
664  data_n[2] = input_->points[nghbr].data[2];
665  data_n[3] = input_->points[nghbr].data[3];
666  Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
667  Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
668  Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal));
669  float residual = fabsf (initial_normal.dot (initial_point - nghbr_point));
670  if (residual > residual_threshold_)
671  is_a_seed = false;
672  }
673 
674  return (true);
675 }
676 
677 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
678 template <typename PointT, typename NormalT> void
680 {
681  cluster.indices.clear ();
682 
683  bool segmentation_is_possible = initCompute ();
684  if ( !segmentation_is_possible )
685  {
686  deinitCompute ();
687  return;
688  }
689 
690  // first of all we need to find out if this point belongs to cloud
691  bool point_was_found = false;
692  int number_of_points = static_cast <int> (indices_->size ());
693  for (size_t point = 0; point < number_of_points; point++)
694  if ( (*indices_)[point] == index)
695  {
696  point_was_found = true;
697  break;
698  }
699 
700  if (point_was_found)
701  {
702  if (clusters_.empty ())
703  {
704  clusters_.clear ();
705  point_neighbours_.clear ();
706  point_labels_.clear ();
707  num_pts_in_segment_.clear ();
708  point_distances_.clear ();
709  segment_neighbours_.clear ();
710  segment_distances_.clear ();
711  segment_labels_.clear ();
712  number_of_segments_ = 0;
713 
714  segmentation_is_possible = prepareForSegmentation ();
715  if ( !segmentation_is_possible )
716  {
717  deinitCompute ();
718  return;
719  }
720 
721  findPointNeighbours ();
722  applySmoothRegionGrowingAlgorithm ();
724 
725  findSegmentNeighbours ();
726  applyRegionMergingAlgorithm ();
727  }
728  // if we have already made the segmentation, then find the segment
729  // to which this point belongs
730  std::vector <pcl::PointIndices>::iterator i_segment;
731  for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++)
732  {
733  bool segment_was_found = false;
734  for (size_t i_point = 0; i_point < i_segment->indices.size (); i_point++)
735  {
736  if (i_segment->indices[i_point] == index)
737  {
738  segment_was_found = true;
739  cluster.indices.clear ();
740  cluster.indices.reserve (i_segment->indices.size ());
741  std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices));
742  break;
743  }
744  }
745  if (segment_was_found)
746  {
747  break;
748  }
749  }// next segment
750  }// end if point was found
751 
752  deinitCompute ();
753 }
754 
755 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article &quot;Color-based segmentation of ...
float getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
RegionGrowingRGB()
Constructor that sets default values for member variables.
virtual ~RegionGrowingRGB()
Destructor that frees memory.
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 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.
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use...
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
float calculateColorimetricalDifference(std::vector< unsigned int > &first_color, std::vector< unsigned int > &second_color) const
This method calculates the colorimetrical difference between two points.
float getDistanceThreshold() const
Returns the distance threshold.
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the 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. ...
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.
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.