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_