Point Cloud Library (PCL)  1.13.0-dev
region_growing_rgb.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the copyright holder(s) nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author : Sergey Ushakov
36  * Email : mine_all_mine@bk.ru
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
41 #define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
42 
43 #include <pcl/console/print.h> // for PCL_ERROR
44 #include <pcl/segmentation/region_growing_rgb.h>
45 #include <pcl/search/search.h>
46 #include <pcl/search/kdtree.h>
47 
48 #include <queue>
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointT, typename NormalT>
53  color_p2p_threshold_ (1225.0f),
54  color_r2r_threshold_ (10.0f),
55  distance_threshold_ (0.05f),
56  region_neighbour_number_ (100),
57  point_distances_ (0),
58  segment_neighbours_ (0),
59  segment_distances_ (0),
60  segment_labels_ (0)
61 {
62  normal_flag_ = false;
63  curvature_flag_ = false;
64  residual_flag_ = false;
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69 template <typename PointT, typename NormalT>
71 {
72  point_distances_.clear ();
73  segment_neighbours_.clear ();
74  segment_distances_.clear ();
75  segment_labels_.clear ();
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT, typename NormalT> float
81 {
82  return (powf (color_p2p_threshold_, 0.5f));
83 }
84 
85 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename PointT, typename NormalT> void
88 {
89  color_p2p_threshold_ = thresh * thresh;
90 }
91 
92 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
93 template <typename PointT, typename NormalT> float
95 {
96  return (powf (color_r2r_threshold_, 0.5f));
97 }
98 
99 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
100 template <typename PointT, typename NormalT> void
102 {
103  color_r2r_threshold_ = thresh * thresh;
104 }
105 
106 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
107 template <typename PointT, typename NormalT> float
109 {
110  return (powf (distance_threshold_, 0.5f));
111 }
112 
113 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
114 template <typename PointT, typename NormalT> void
116 {
117  distance_threshold_ = thresh * thresh;
118 }
119 
120 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
121 template <typename PointT, typename NormalT> unsigned int
123 {
124  return (region_neighbour_number_);
125 }
126 
127 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
128 template <typename PointT, typename NormalT> void
130 {
131  region_neighbour_number_ = nghbr_number;
132 }
133 
134 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
135 template <typename PointT, typename NormalT> bool
137 {
138  return (normal_flag_);
139 }
140 
141 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
142 template <typename PointT, typename NormalT> void
144 {
145  normal_flag_ = value;
146 }
147 
148 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
149 template <typename PointT, typename NormalT> void
151 {
152  curvature_flag_ = value;
153 }
154 
155 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
156 template <typename PointT, typename NormalT> void
158 {
159  residual_flag_ = value;
160 }
161 
162 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
163 template <typename PointT, typename NormalT> void
164 pcl::RegionGrowingRGB<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
165 {
166  clusters_.clear ();
167  clusters.clear ();
168  point_neighbours_.clear ();
169  point_labels_.clear ();
170  num_pts_in_segment_.clear ();
171  point_distances_.clear ();
172  segment_neighbours_.clear ();
173  segment_distances_.clear ();
174  segment_labels_.clear ();
175  number_of_segments_ = 0;
176 
177  bool segmentation_is_possible = initCompute ();
178  if ( !segmentation_is_possible )
179  {
180  deinitCompute ();
181  return;
182  }
183 
184  segmentation_is_possible = prepareForSegmentation ();
185  if ( !segmentation_is_possible )
186  {
187  deinitCompute ();
188  return;
189  }
190 
191  findPointNeighbours ();
192  applySmoothRegionGrowingAlgorithm ();
194 
195  findSegmentNeighbours ();
196  applyRegionMergingAlgorithm ();
197 
198  auto cluster_iter = clusters_.begin ();
199  while (cluster_iter != clusters_.end ())
200  {
201  if (cluster_iter->indices.size () < min_pts_per_cluster_ ||
202  cluster_iter->indices.size () > max_pts_per_cluster_)
203  {
204  cluster_iter = clusters_.erase (cluster_iter);
205  }
206  else
207  ++cluster_iter;
208  }
209 
210  clusters.reserve (clusters_.size ());
211  std::copy (clusters_.cbegin (), clusters_.cend (), std::back_inserter (clusters));
212 
213  deinitCompute ();
214 }
215 
216 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
217 template <typename PointT, typename NormalT> bool
219 {
220  // if user forgot to pass point cloud or if it is empty
221  if ( input_->points.empty () )
222  return (false);
223 
224  // if normal/smoothness test is on then we need to check if all needed variables and parameters
225  // were correctly initialized
226  if (normal_flag_)
227  {
228  // if user forgot to pass normals or the sizes of point and normal cloud are different
229  if ( !normals_ || input_->size () != normals_->size () )
230  return (false);
231  }
232 
233  // if residual test is on then we need to check if all needed parameters were correctly initialized
234  if (residual_flag_)
235  {
236  if (residual_threshold_ <= 0.0f)
237  return (false);
238  }
239 
240  // if curvature test is on ...
241  // if (curvature_flag_)
242  // {
243  // in this case we do not need to check anything that related to it
244  // so we simply commented it
245  // }
246 
247  // here we check the parameters related to color-based segmentation
248  if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f )
249  return (false);
250 
251  // from here we check those parameters that are always valuable
252  if (neighbour_number_ == 0)
253  return (false);
254 
255  // if user didn't set search method
256  if (!search_)
257  search_.reset (new pcl::search::KdTree<PointT>);
258 
259  if (indices_)
260  {
261  if (indices_->empty ())
262  PCL_ERROR ("[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
263  search_->setInputCloud (input_, indices_);
264  }
265  else
266  search_->setInputCloud (input_);
267 
268  return (true);
269 }
270 
271 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
272 template <typename PointT, typename NormalT> void
274 {
275  int point_number = static_cast<int> (indices_->size ());
276  pcl::Indices neighbours;
277  std::vector<float> distances;
278 
279  point_neighbours_.resize (input_->size (), neighbours);
280  point_distances_.resize (input_->size (), distances);
281 
282  for (int i_point = 0; i_point < point_number; i_point++)
283  {
284  int point_index = (*indices_)[i_point];
285  neighbours.clear ();
286  distances.clear ();
287  search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances);
288  point_neighbours_[point_index].swap (neighbours);
289  point_distances_[point_index].swap (distances);
290  }
291 }
292 
293 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
294 template <typename PointT, typename NormalT> void
296 {
297  pcl::Indices neighbours;
298  std::vector<float> distances;
299  segment_neighbours_.resize (number_of_segments_, neighbours);
300  segment_distances_.resize (number_of_segments_, distances);
301 
302  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
303  {
304  pcl::Indices nghbrs;
305  std::vector<float> dist;
306  findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
307  segment_neighbours_[i_seg].swap (nghbrs);
308  segment_distances_[i_seg].swap (dist);
309  }
310 }
311 
312 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
313 template <typename PointT,typename NormalT> void
315 {
316  std::vector<float> distances;
317  float max_dist = std::numeric_limits<float>::max ();
318  distances.resize (clusters_.size (), max_dist);
319 
320  const auto number_of_points = num_pts_in_segment_[index];
321  //loop through every point in this segment and check neighbours
322  for (pcl::uindex_t i_point = 0; i_point < number_of_points; i_point++)
323  {
324  const auto point_index = clusters_[index].indices[i_point];
325  const auto number_of_neighbours = point_neighbours_[point_index].size ();
326  //loop through every neighbour of the current point, find out to which segment it belongs
327  //and if it belongs to neighbouring segment and is close enough then remember segment and its distance
328  for (std::size_t i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
329  {
330  // find segment
331  const pcl::index_t 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 (segment_neighbours.size () > nghbr_number)
349  segment_neighbours.pop ();
350  }
351  }
352 
353  const std::size_t size = std::min<std::size_t> (segment_neighbours.size (), static_cast<std::size_t>(nghbr_number));
354  nghbrs.resize (size, 0);
355  dist.resize (size, 0);
356  pcl::uindex_t 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  // calculate color of each segment
371  std::vector< std::vector<unsigned int> > segment_color;
372  std::vector<unsigned int> color;
373  color.resize (3, 0);
374  segment_color.resize (number_of_segments_, color);
375 
376  for (const auto& point_index : (*indices_))
377  {
378  int segment_index = point_labels_[point_index];
379  segment_color[segment_index][0] += (*input_)[point_index].r;
380  segment_color[segment_index][1] += (*input_)[point_index].g;
381  segment_color[segment_index][2] += (*input_)[point_index].b;
382  }
383  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
384  {
385  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]));
386  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]));
387  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]));
388  }
389 
390  // now it is time to find out if there are segments with a similar color
391  // and merge them together
392  std::vector<unsigned int> num_pts_in_homogeneous_region;
393  std::vector<int> num_seg_in_homogeneous_region;
394 
395  segment_labels_.resize (number_of_segments_, -1);
396 
397  float dist_thresh = distance_threshold_;
398  int homogeneous_region_number = 0;
399  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
400  {
401  int curr_homogeneous_region = 0;
402  if (segment_labels_[i_seg] == -1)
403  {
404  segment_labels_[i_seg] = homogeneous_region_number;
405  curr_homogeneous_region = homogeneous_region_number;
406  num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
407  num_seg_in_homogeneous_region.push_back (1);
408  homogeneous_region_number++;
409  }
410  else
411  curr_homogeneous_region = segment_labels_[i_seg];
412 
413  unsigned int i_nghbr = 0;
414  while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
415  {
416  int index = segment_neighbours_[i_seg][i_nghbr];
417  if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
418  {
419  i_nghbr++;
420  continue;
421  }
422  if ( segment_labels_[index] == -1 )
423  {
424  float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
425  if (difference < color_r2r_threshold_)
426  {
427  segment_labels_[index] = curr_homogeneous_region;
428  num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
429  num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
430  }
431  }
432  i_nghbr++;
433  }// next neighbour
434  }// next segment
435 
436  segment_color.clear ();
437  color.clear ();
438 
439  std::vector< std::vector<int> > final_segments;
440  std::vector<int> region;
441  final_segments.resize (homogeneous_region_number, region);
442  for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
443  {
444  final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
445  }
446 
447  std::vector<int> counter;
448  counter.resize (homogeneous_region_number, 0);
449  for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
450  {
451  int index = segment_labels_[i_seg];
452  final_segments[ index ][ counter[index] ] = i_seg;
453  counter[index] += 1;
454  }
455 
456  std::vector< std::vector< std::pair<float, pcl::index_t> > > region_neighbours;
457  findRegionNeighbours (region_neighbours, final_segments);
458 
459  int final_segment_number = homogeneous_region_number;
460  for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
461  {
462  if (num_pts_in_homogeneous_region[i_reg] < min_pts_per_cluster_)
463  {
464  if ( region_neighbours[i_reg].empty () )
465  continue;
466  int nearest_neighbour = region_neighbours[i_reg][0].second;
467  if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
468  continue;
469  int reg_index = segment_labels_[nearest_neighbour];
470  int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
471  for (int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
472  {
473  int segment_index = final_segments[i_reg][i_seg];
474  final_segments[reg_index].push_back (segment_index);
475  segment_labels_[segment_index] = reg_index;
476  }
477  final_segments[i_reg].clear ();
478  num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
479  num_pts_in_homogeneous_region[i_reg] = 0;
480  num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
481  num_seg_in_homogeneous_region[i_reg] = 0;
482  final_segment_number -= 1;
483 
484  for (auto& nghbr : region_neighbours[reg_index])
485  {
486  if ( segment_labels_[ nghbr.second ] == reg_index )
487  {
488  nghbr.first = std::numeric_limits<float>::max ();
489  nghbr.second = 0;
490  }
491  }
492  for (const auto& nghbr : region_neighbours[i_reg])
493  {
494  if ( segment_labels_[ nghbr.second ] != reg_index )
495  {
496  region_neighbours[reg_index].push_back (nghbr);
497  }
498  }
499  region_neighbours[i_reg].clear ();
500  std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (), comparePair);
501  }
502  }
503 
504  assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ()));
505 
506  number_of_segments_ = final_segment_number;
507 }
508 
509 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
510 template <typename PointT, typename NormalT> float
511 pcl::RegionGrowingRGB<PointT, NormalT>::calculateColorimetricalDifference (std::vector<unsigned int>& first_color, std::vector<unsigned int>& second_color) const
512 {
513  float difference = 0.0f;
514  difference += static_cast<float>((first_color[0] - second_color[0]) * (first_color[0] - second_color[0]));
515  difference += static_cast<float>((first_color[1] - second_color[1]) * (first_color[1] - second_color[1]));
516  difference += static_cast<float>((first_color[2] - second_color[2]) * (first_color[2] - second_color[2]));
517  return (difference);
518 }
519 
520 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
521 template <typename PointT, typename NormalT> void
522 pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::vector< std::pair<float, pcl::index_t> > >& neighbours_out, std::vector< std::vector<int> >& regions_in)
523 {
524  int region_number = static_cast<int> (regions_in.size ());
525  neighbours_out.clear ();
526  neighbours_out.resize (region_number);
527 
528  for (int i_reg = 0; i_reg < region_number; i_reg++)
529  {
530  neighbours_out[i_reg].reserve (regions_in[i_reg].size () * region_neighbour_number_);
531  for (const auto& curr_segment : regions_in[i_reg])
532  {
533  const std::size_t nghbr_number = segment_neighbours_[curr_segment].size ();
534  std::pair<float, pcl::index_t> pair;
535  for (std::size_t i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
536  {
537  const auto segment_index = segment_neighbours_[curr_segment][i_nghbr];
538  if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
539  continue;
540  if (segment_labels_[segment_index] != i_reg)
541  {
542  pair.first = segment_distances_[curr_segment][i_nghbr];
543  pair.second = segment_index;
544  neighbours_out[i_reg].push_back (pair);
545  }
546  }// next neighbour
547  }// next segment
548  std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (), comparePair);
549  }// next homogeneous region
550 }
551 
552 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
553 template <typename PointT, typename NormalT> void
554 pcl::RegionGrowingRGB<PointT, NormalT>::assembleRegions (std::vector<unsigned int>& num_pts_in_region, int num_regions)
555 {
556  clusters_.clear ();
557  pcl::PointIndices segment;
558  clusters_.resize (num_regions, segment);
559  for (int i_seg = 0; i_seg < num_regions; i_seg++)
560  {
561  clusters_[i_seg].indices.resize (num_pts_in_region[i_seg]);
562  }
563 
564  std::vector<int> counter;
565  counter.resize (num_regions, 0);
566  for (const auto& point_index : (*indices_))
567  {
568  int index = point_labels_[point_index];
569  index = segment_labels_[index];
570  clusters_[index].indices[ counter[index] ] = point_index;
571  counter[index] += 1;
572  }
573 
574  // now we need to erase empty regions
575  if (clusters_.empty ())
576  return;
577 
578  std::vector<pcl::PointIndices>::iterator itr1, itr2;
579  itr1 = clusters_.begin ();
580  itr2 = clusters_.end () - 1;
581 
582  while (itr1 < itr2)
583  {
584  while (!(itr1->indices.empty ()) && itr1 < itr2)
585  ++itr1;
586  while ( itr2->indices.empty () && itr1 < itr2)
587  --itr2;
588 
589  if (itr1 != itr2)
590  itr1->indices.swap (itr2->indices);
591  }
592 
593  if (itr2->indices.empty ())
594  clusters_.erase (itr2, clusters_.end ());
595 }
596 
597 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
598 template <typename PointT, typename NormalT> bool
600 {
601  is_a_seed = true;
602 
603  // check the color difference
604  std::vector<unsigned int> point_color;
605  point_color.resize (3, 0);
606  std::vector<unsigned int> nghbr_color;
607  nghbr_color.resize (3, 0);
608  point_color[0] = (*input_)[point].r;
609  point_color[1] = (*input_)[point].g;
610  point_color[2] = (*input_)[point].b;
611  nghbr_color[0] = (*input_)[nghbr].r;
612  nghbr_color[1] = (*input_)[nghbr].g;
613  nghbr_color[2] = (*input_)[nghbr].b;
614  float difference = calculateColorimetricalDifference (point_color, nghbr_color);
615  if (difference > color_p2p_threshold_)
616  return (false);
617 
618  float cosine_threshold = std::cos (theta_threshold_);
619 
620  // check the angle between normals if needed
621  if (normal_flag_)
622  {
623  float data[4];
624  data[0] = (*input_)[point].data[0];
625  data[1] = (*input_)[point].data[1];
626  data[2] = (*input_)[point].data[2];
627  data[3] = (*input_)[point].data[3];
628 
629  Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
630  Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
631  if (smooth_mode_flag_ == true)
632  {
633  Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
634  float dot_product = std::abs (nghbr_normal.dot (initial_normal));
635  if (dot_product < cosine_threshold)
636  return (false);
637  }
638  else
639  {
640  Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
641  Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> ((*normals_)[initial_seed].normal));
642  float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
643  if (dot_product < cosine_threshold)
644  return (false);
645  }
646  }
647 
648  // check the curvature if needed
649  if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
650  is_a_seed = false;
651 
652  // check the residual if needed
653  if (residual_flag_)
654  {
655  float data_p[4];
656  data_p[0] = (*input_)[point].data[0];
657  data_p[1] = (*input_)[point].data[1];
658  data_p[2] = (*input_)[point].data[2];
659  data_p[3] = (*input_)[point].data[3];
660  float data_n[4];
661  data_n[0] = (*input_)[nghbr].data[0];
662  data_n[1] = (*input_)[nghbr].data[1];
663  data_n[2] = (*input_)[nghbr].data[2];
664  data_n[3] = (*input_)[nghbr].data[3];
665  Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
666  Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
667  Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
668  float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
669  if (residual > residual_threshold_)
670  is_a_seed = false;
671  }
672 
673  return (true);
674 }
675 
676 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
677 template <typename PointT, typename NormalT> void
679 {
680  cluster.indices.clear ();
681 
682  bool segmentation_is_possible = initCompute ();
683  if ( !segmentation_is_possible )
684  {
685  deinitCompute ();
686  return;
687  }
688 
689  // first of all we need to find out if this point belongs to cloud
690  bool point_was_found = false;
691  for (const auto& point : (*indices_))
692  if (point == index)
693  {
694  point_was_found = true;
695  break;
696  }
697 
698  if (point_was_found)
699  {
700  if (clusters_.empty ())
701  {
702  clusters_.clear ();
703  point_neighbours_.clear ();
704  point_labels_.clear ();
705  num_pts_in_segment_.clear ();
706  point_distances_.clear ();
707  segment_neighbours_.clear ();
708  segment_distances_.clear ();
709  segment_labels_.clear ();
710  number_of_segments_ = 0;
711 
712  segmentation_is_possible = prepareForSegmentation ();
713  if ( !segmentation_is_possible )
714  {
715  deinitCompute ();
716  return;
717  }
718 
719  findPointNeighbours ();
720  applySmoothRegionGrowingAlgorithm ();
722 
723  findSegmentNeighbours ();
724  applyRegionMergingAlgorithm ();
725  }
726  // if we have already made the segmentation, then find the segment
727  // to which this point belongs
728  for (const auto& i_segment : clusters_)
729  {
730  const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
731  if (it != i_segment.indices.cend())
732  {
733  // if segment was found
734  cluster.indices.clear ();
735  cluster.indices.reserve (i_segment.indices.size ());
736  std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.indices));
737  break;
738  }
739  }// next segment
740  }// end if point was found
741 
742  deinitCompute ();
743 }
744 
745 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_
bool curvature_flag_
If set to true then curvature test will be done during segmentation.
bool normal_flag_
If set to true then normal/smoothness test will be done during segmentation.
bool residual_flag_
If set to true then residual test will be done during segmentation.
pcl::uindex_t min_pts_per_cluster_
Stores the minimum number of points that a cluster needs to contain in order to be considered valid.
void assembleRegions()
This function simply assembles the regions from list of point labels.
void getSegmentFromPoint(index_t index, pcl::PointIndices &cluster) override
For a given point this function builds a segment to which it belongs and returns this segment.
void setDistanceThreshold(float thresh)
Allows to set distance threshold.
void setRegionColorThreshold(float thresh)
This method specifies the threshold value for color test between the regions.
float getPointColorThreshold() const
Returns the color threshold value used for testing if points belong to the same region.
bool prepareForSegmentation() override
This method simply checks if it is possible to execute the segmentation algorithm with the current se...
void findRegionNeighbours(std::vector< std::vector< std::pair< float, pcl::index_t > > > &neighbours_out, std::vector< std::vector< int > > &regions_in)
This method assembles the array containing neighbours of each homogeneous region.
void applyRegionMergingAlgorithm()
This function implements the merging algorithm described in the article "Color-based segmentation of ...
unsigned int getNumberOfRegionNeighbours() const
Returns the number of nearest neighbours used for searching K nearest segments.
void setCurvatureTestFlag(bool value) override
Allows to turn on/off the curvature test.
void extract(std::vector< pcl::PointIndices > &clusters) override
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
void setNormalTestFlag(bool value)
Allows to turn on/off the smoothness test.
void setNumberOfRegionNeighbours(unsigned int nghbr_number)
This method allows to set the number of neighbours that is used for finding neighbouring 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.
float getDistanceThreshold() const
Returns the distance threshold.
bool getNormalTestFlag() const
Returns the flag that signalize if the smoothness test is turned on/off.
void findRegionsKNN(pcl::index_t index, pcl::uindex_t nghbr_number, Indices &nghbrs, std::vector< float > &dist)
This method finds K nearest neighbours of the given segment.
RegionGrowingRGB()
Constructor that sets default values for member variables.
void setResidualTestFlag(bool value) override
Allows to turn on/off the residual test.
~RegionGrowingRGB() override
Destructor that frees memory.
void findPointNeighbours() override
This method finds KNN for each point and saves them to the array because the algorithm needs to find ...
float getRegionColorThreshold() const
Returns the color threshold value used for testing if regions can be merged.
void findSegmentNeighbours()
This method simply calls the findRegionsKNN for each segment and saves the results for later use.
void setPointColorThreshold(float thresh)
This method specifies the threshold value for color test between the points.
bool validatePoint(index_t initial_seed, index_t point, index_t nghbr, bool &is_a_seed) const override
This function is checking if the point with index 'nghbr' belongs to the segment.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
bool comparePair(std::pair< float, int > i, std::pair< float, int > j)
This function is used as a comparator for sorting.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133