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 #pragma once
41 
42 #include <pcl/segmentation/region_growing.h>
43 
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_types.h>
46 #include <pcl/common/point_tests.h> // for pcl::isFinite
47 #include <pcl/console/print.h> // for PCL_ERROR
48 #include <pcl/search/search.h>
49 #include <pcl/search/kdtree.h>
50 
51 #include <queue>
52 #include <cmath>
53 #include <ctime>
54 
55 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
56 template <typename PointT, typename NormalT>
RegionGrowing()57 pcl::RegionGrowing<PointT, NormalT>::RegionGrowing () :
58   min_pts_per_cluster_ (1),
59   max_pts_per_cluster_ (std::numeric_limits<pcl::uindex_t>::max ()),
60   smooth_mode_flag_ (true),
61   curvature_flag_ (true),
62   residual_flag_ (false),
63   theta_threshold_ (30.0f / 180.0f * static_cast<float> (M_PI)),
64   residual_threshold_ (0.05f),
65   curvature_threshold_ (0.05f),
66   neighbour_number_ (30),
67   search_ (),
68   normals_ (),
69   point_neighbours_ (0),
70   point_labels_ (0),
71   normal_flag_ (true),
72   num_pts_in_segment_ (0),
73   clusters_ (0),
74   number_of_segments_ (0)
75 {
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT, typename NormalT>
~RegionGrowing()80 pcl::RegionGrowing<PointT, NormalT>::~RegionGrowing ()
81 {
82   point_neighbours_.clear ();
83   point_labels_.clear ();
84   num_pts_in_segment_.clear ();
85   clusters_.clear ();
86 }
87 
88 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
89 template <typename PointT, typename NormalT> pcl::uindex_t
getMinClusterSize()90 pcl::RegionGrowing<PointT, NormalT>::getMinClusterSize ()
91 {
92   return (min_pts_per_cluster_);
93 }
94 
95 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
96 template <typename PointT, typename NormalT> void
setMinClusterSize(pcl::uindex_t min_cluster_size)97 pcl::RegionGrowing<PointT, NormalT>::setMinClusterSize (pcl::uindex_t min_cluster_size)
98 {
99   min_pts_per_cluster_ = min_cluster_size;
100 }
101 
102 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
103 template <typename PointT, typename NormalT> pcl::uindex_t
getMaxClusterSize()104 pcl::RegionGrowing<PointT, NormalT>::getMaxClusterSize ()
105 {
106   return (max_pts_per_cluster_);
107 }
108 
109 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
110 template <typename PointT, typename NormalT> void
setMaxClusterSize(pcl::uindex_t max_cluster_size)111 pcl::RegionGrowing<PointT, NormalT>::setMaxClusterSize (pcl::uindex_t max_cluster_size)
112 {
113   max_pts_per_cluster_ = max_cluster_size;
114 }
115 
116 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
117 template <typename PointT, typename NormalT> bool
getSmoothModeFlag() const118 pcl::RegionGrowing<PointT, NormalT>::getSmoothModeFlag () const
119 {
120   return (smooth_mode_flag_);
121 }
122 
123 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
124 template <typename PointT, typename NormalT> void
setSmoothModeFlag(bool value)125 pcl::RegionGrowing<PointT, NormalT>::setSmoothModeFlag (bool value)
126 {
127   smooth_mode_flag_ = value;
128 }
129 
130 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
131 template <typename PointT, typename NormalT> bool
getCurvatureTestFlag() const132 pcl::RegionGrowing<PointT, NormalT>::getCurvatureTestFlag () const
133 {
134   return (curvature_flag_);
135 }
136 
137 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
138 template <typename PointT, typename NormalT> void
setCurvatureTestFlag(bool value)139 pcl::RegionGrowing<PointT, NormalT>::setCurvatureTestFlag (bool value)
140 {
141   curvature_flag_ = value;
142 
143   if (curvature_flag_ == false && residual_flag_ == false)
144     residual_flag_ = true;
145 }
146 
147 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
148 template <typename PointT, typename NormalT> bool
getResidualTestFlag() const149 pcl::RegionGrowing<PointT, NormalT>::getResidualTestFlag () const
150 {
151   return (residual_flag_);
152 }
153 
154 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
155 template <typename PointT, typename NormalT> void
setResidualTestFlag(bool value)156 pcl::RegionGrowing<PointT, NormalT>::setResidualTestFlag (bool value)
157 {
158   residual_flag_ = value;
159 
160   if (curvature_flag_ == false && residual_flag_ == false)
161     curvature_flag_ = true;
162 }
163 
164 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
165 template <typename PointT, typename NormalT> float
getSmoothnessThreshold() const166 pcl::RegionGrowing<PointT, NormalT>::getSmoothnessThreshold () const
167 {
168   return (theta_threshold_);
169 }
170 
171 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
172 template <typename PointT, typename NormalT> void
setSmoothnessThreshold(float theta)173 pcl::RegionGrowing<PointT, NormalT>::setSmoothnessThreshold (float theta)
174 {
175   theta_threshold_ = theta;
176 }
177 
178 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
179 template <typename PointT, typename NormalT> float
getResidualThreshold() const180 pcl::RegionGrowing<PointT, NormalT>::getResidualThreshold () const
181 {
182   return (residual_threshold_);
183 }
184 
185 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
186 template <typename PointT, typename NormalT> void
setResidualThreshold(float residual)187 pcl::RegionGrowing<PointT, NormalT>::setResidualThreshold (float residual)
188 {
189   residual_threshold_ = residual;
190 }
191 
192 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
193 template <typename PointT, typename NormalT> float
getCurvatureThreshold() const194 pcl::RegionGrowing<PointT, NormalT>::getCurvatureThreshold () const
195 {
196   return (curvature_threshold_);
197 }
198 
199 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
200 template <typename PointT, typename NormalT> void
setCurvatureThreshold(float curvature)201 pcl::RegionGrowing<PointT, NormalT>::setCurvatureThreshold (float curvature)
202 {
203   curvature_threshold_ = curvature;
204 }
205 
206 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
207 template <typename PointT, typename NormalT> unsigned int
getNumberOfNeighbours() const208 pcl::RegionGrowing<PointT, NormalT>::getNumberOfNeighbours () const
209 {
210   return (neighbour_number_);
211 }
212 
213 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
214 template <typename PointT, typename NormalT> void
setNumberOfNeighbours(unsigned int neighbour_number)215 pcl::RegionGrowing<PointT, NormalT>::setNumberOfNeighbours (unsigned int neighbour_number)
216 {
217   neighbour_number_ = neighbour_number;
218 }
219 
220 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
221 template <typename PointT, typename NormalT> typename pcl::RegionGrowing<PointT, NormalT>::KdTreePtr
getSearchMethod() const222 pcl::RegionGrowing<PointT, NormalT>::getSearchMethod () const
223 {
224   return (search_);
225 }
226 
227 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
228 template <typename PointT, typename NormalT> void
setSearchMethod(const KdTreePtr & tree)229 pcl::RegionGrowing<PointT, NormalT>::setSearchMethod (const KdTreePtr& tree)
230 {
231   search_ = tree;
232 }
233 
234 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
235 template <typename PointT, typename NormalT> typename pcl::RegionGrowing<PointT, NormalT>::NormalPtr
getInputNormals() const236 pcl::RegionGrowing<PointT, NormalT>::getInputNormals () const
237 {
238   return (normals_);
239 }
240 
241 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
242 template <typename PointT, typename NormalT> void
setInputNormals(const NormalPtr & norm)243 pcl::RegionGrowing<PointT, NormalT>::setInputNormals (const NormalPtr& norm)
244 {
245   normals_ = norm;
246 }
247 
248 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
249 template <typename PointT, typename NormalT> void
extract(std::vector<pcl::PointIndices> & clusters)250 pcl::RegionGrowing<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
251 {
252   clusters_.clear ();
253   clusters.clear ();
254   point_neighbours_.clear ();
255   point_labels_.clear ();
256   num_pts_in_segment_.clear ();
257   number_of_segments_ = 0;
258 
259   bool segmentation_is_possible = initCompute ();
260   if ( !segmentation_is_possible )
261   {
262     deinitCompute ();
263     return;
264   }
265 
266   segmentation_is_possible = prepareForSegmentation ();
267   if ( !segmentation_is_possible )
268   {
269     deinitCompute ();
270     return;
271   }
272 
273   findPointNeighbours ();
274   applySmoothRegionGrowingAlgorithm ();
275   assembleRegions ();
276 
277   clusters.resize (clusters_.size ());
278   std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin ();
279   for (const auto& cluster : clusters_)
280   {
281     if ((cluster.indices.size () >= min_pts_per_cluster_) &&
282         (cluster.indices.size () <= max_pts_per_cluster_))
283     {
284       *cluster_iter_input = cluster;
285       ++cluster_iter_input;
286     }
287   }
288 
289   clusters_ = std::vector<pcl::PointIndices> (clusters.begin (), cluster_iter_input);
290   clusters.resize(clusters_.size());
291 
292   deinitCompute ();
293 }
294 
295 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
296 template <typename PointT, typename NormalT> bool
prepareForSegmentation()297 pcl::RegionGrowing<PointT, NormalT>::prepareForSegmentation ()
298 {
299   // if user forgot to pass point cloud or if it is empty
300   if ( input_->points.empty () )
301     return (false);
302 
303   // if user forgot to pass normals or the sizes of point and normal cloud are different
304   if ( !normals_ || input_->size () != normals_->size () )
305     return (false);
306 
307   // if residual test is on then we need to check if all needed parameters were correctly initialized
308   if (residual_flag_)
309   {
310     if (residual_threshold_ <= 0.0f)
311       return (false);
312   }
313 
314   // if curvature test is on ...
315   // if (curvature_flag_)
316   // {
317   //   in this case we do not need to check anything that related to it
318   //   so we simply commented it
319   // }
320 
321   // from here we check those parameters that are always valuable
322   if (neighbour_number_ == 0)
323     return (false);
324 
325   // if user didn't set search method
326   if (!search_)
327     search_.reset (new pcl::search::KdTree<PointT>);
328 
329   if (indices_)
330   {
331     if (indices_->empty ())
332       PCL_ERROR ("[pcl::RegionGrowing::prepareForSegmentation] Empty given indices!\n");
333     search_->setInputCloud (input_, indices_);
334   }
335   else
336     search_->setInputCloud (input_);
337 
338   return (true);
339 }
340 
341 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342 template <typename PointT, typename NormalT> void
findPointNeighbours()343 pcl::RegionGrowing<PointT, NormalT>::findPointNeighbours ()
344 {
345   int point_number = static_cast<int> (indices_->size ());
346   pcl::Indices neighbours;
347   std::vector<float> distances;
348 
349   point_neighbours_.resize (input_->size (), neighbours);
350   if (input_->is_dense)
351   {
352     for (int i_point = 0; i_point < point_number; i_point++)
353     {
354       int point_index = (*indices_)[i_point];
355       neighbours.clear ();
356       search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
357       point_neighbours_[point_index].swap (neighbours);
358     }
359   }
360   else
361   {
362     for (int i_point = 0; i_point < point_number; i_point++)
363     {
364       neighbours.clear ();
365       int point_index = (*indices_)[i_point];
366       if (!pcl::isFinite ((*input_)[point_index]))
367         continue;
368       search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
369       point_neighbours_[point_index].swap (neighbours);
370     }
371   }
372 }
373 
374 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
375 template <typename PointT, typename NormalT> void
applySmoothRegionGrowingAlgorithm()376 pcl::RegionGrowing<PointT, NormalT>::applySmoothRegionGrowingAlgorithm ()
377 {
378   int num_of_pts = static_cast<int> (indices_->size ());
379   point_labels_.resize (input_->size (), -1);
380 
381   std::vector< std::pair<float, int> > point_residual;
382   std::pair<float, int> pair;
383   point_residual.resize (num_of_pts, pair);
384 
385   if (normal_flag_ == true)
386   {
387     for (int i_point = 0; i_point < num_of_pts; i_point++)
388     {
389       int point_index = (*indices_)[i_point];
390       point_residual[i_point].first = (*normals_)[point_index].curvature;
391       point_residual[i_point].second = point_index;
392     }
393     std::sort (point_residual.begin (), point_residual.end (), comparePair);
394   }
395   else
396   {
397     for (int i_point = 0; i_point < num_of_pts; i_point++)
398     {
399       int point_index = (*indices_)[i_point];
400       point_residual[i_point].first = 0;
401       point_residual[i_point].second = point_index;
402     }
403   }
404   int seed_counter = 0;
405   int seed = point_residual[seed_counter].second;
406 
407   int segmented_pts_num = 0;
408   int number_of_segments = 0;
409   while (segmented_pts_num < num_of_pts)
410   {
411     int pts_in_segment;
412     pts_in_segment = growRegion (seed, number_of_segments);
413     segmented_pts_num += pts_in_segment;
414     num_pts_in_segment_.push_back (pts_in_segment);
415     number_of_segments++;
416 
417     //find next point that is not segmented yet
418     for (int i_seed = seed_counter + 1; i_seed < num_of_pts; i_seed++)
419     {
420       int index = point_residual[i_seed].second;
421       if (point_labels_[index] == -1)
422       {
423         seed = index;
424         seed_counter = i_seed;
425         break;
426       }
427     }
428   }
429 }
430 
431 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
432 template <typename PointT, typename NormalT> int
growRegion(int initial_seed,int segment_number)433 pcl::RegionGrowing<PointT, NormalT>::growRegion (int initial_seed, int segment_number)
434 {
435   std::queue<int> seeds;
436   seeds.push (initial_seed);
437   point_labels_[initial_seed] = segment_number;
438 
439   int num_pts_in_segment = 1;
440 
441   while (!seeds.empty ())
442   {
443     int curr_seed;
444     curr_seed = seeds.front ();
445     seeds.pop ();
446 
447     std::size_t i_nghbr = 0;
448     while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () )
449     {
450       int index = point_neighbours_[curr_seed][i_nghbr];
451       if (point_labels_[index] != -1)
452       {
453         i_nghbr++;
454         continue;
455       }
456 
457       bool is_a_seed = false;
458       bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed);
459 
460       if (!belongs_to_segment)
461       {
462         i_nghbr++;
463         continue;
464       }
465 
466       point_labels_[index] = segment_number;
467       num_pts_in_segment++;
468 
469       if (is_a_seed)
470       {
471         seeds.push (index);
472       }
473 
474       i_nghbr++;
475     }// next neighbour
476   }// next seed
477 
478   return (num_pts_in_segment);
479 }
480 
481 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
482 template <typename PointT, typename NormalT> bool
validatePoint(pcl::index_t initial_seed,pcl::index_t point,pcl::index_t nghbr,bool & is_a_seed) const483 pcl::RegionGrowing<PointT, NormalT>::validatePoint (pcl::index_t initial_seed, pcl::index_t point, pcl::index_t nghbr, bool& is_a_seed) const
484 {
485   is_a_seed = true;
486 
487   float cosine_threshold = std::cos (theta_threshold_);
488   float data[4];
489 
490   data[0] = (*input_)[point].data[0];
491   data[1] = (*input_)[point].data[1];
492   data[2] = (*input_)[point].data[2];
493   data[3] = (*input_)[point].data[3];
494   Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
495   Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
496 
497   //check the angle between normals
498   if (smooth_mode_flag_ == true)
499   {
500     Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
501     float dot_product = std::abs (nghbr_normal.dot (initial_normal));
502     if (dot_product < cosine_threshold)
503     {
504       return (false);
505     }
506   }
507   else
508   {
509     Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
510     Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> ((*normals_)[initial_seed].normal));
511     float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
512     if (dot_product < cosine_threshold)
513       return (false);
514   }
515 
516   // check the curvature if needed
517   if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
518   {
519     is_a_seed = false;
520   }
521 
522   // check the residual if needed
523   float data_1[4];
524 
525   data_1[0] = (*input_)[nghbr].data[0];
526   data_1[1] = (*input_)[nghbr].data[1];
527   data_1[2] = (*input_)[nghbr].data[2];
528   data_1[3] = (*input_)[nghbr].data[3];
529   Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_1));
530   float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
531   if (residual_flag_ && residual > residual_threshold_)
532     is_a_seed = false;
533 
534   return (true);
535 }
536 
537 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
538 template <typename PointT, typename NormalT> void
assembleRegions()539 pcl::RegionGrowing<PointT, NormalT>::assembleRegions ()
540 {
541   const auto number_of_segments = num_pts_in_segment_.size ();
542   const auto number_of_points = input_->size ();
543 
544   pcl::PointIndices segment;
545   clusters_.resize (number_of_segments, segment);
546 
547   for (std::size_t i_seg = 0; i_seg < number_of_segments; i_seg++)
548   {
549     clusters_[i_seg].indices.resize ( num_pts_in_segment_[i_seg], 0);
550   }
551 
552   std::vector<int> counter(number_of_segments, 0);
553 
554   for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
555   {
556     const auto segment_index = point_labels_[i_point];
557     if (segment_index != -1)
558     {
559       const auto point_index = counter[segment_index];
560       clusters_[segment_index].indices[point_index] = i_point;
561       counter[segment_index] = point_index + 1;
562     }
563   }
564 
565   number_of_segments_ = number_of_segments;
566 }
567 
568 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
569 template <typename PointT, typename NormalT> void
getSegmentFromPoint(pcl::index_t index,pcl::PointIndices & cluster)570 pcl::RegionGrowing<PointT, NormalT>::getSegmentFromPoint (pcl::index_t index, pcl::PointIndices& cluster)
571 {
572   cluster.indices.clear ();
573 
574   bool segmentation_is_possible = initCompute ();
575   if ( !segmentation_is_possible )
576   {
577     deinitCompute ();
578     return;
579   }
580 
581   // first of all we need to find out if this point belongs to cloud
582   bool point_was_found = false;
583   for (const auto& point : (*indices_))
584     if (point == index)
585     {
586       point_was_found = true;
587       break;
588     }
589 
590   if (point_was_found)
591   {
592     if (clusters_.empty ())
593     {
594       point_neighbours_.clear ();
595       point_labels_.clear ();
596       num_pts_in_segment_.clear ();
597       number_of_segments_ = 0;
598 
599       segmentation_is_possible = prepareForSegmentation ();
600       if ( !segmentation_is_possible )
601       {
602         deinitCompute ();
603         return;
604       }
605 
606       findPointNeighbours ();
607       applySmoothRegionGrowingAlgorithm ();
608       assembleRegions ();
609     }
610     // if we have already made the segmentation, then find the segment
611     // to which this point belongs
612     for (const auto& i_segment : clusters_)
613     {
614       const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
615       if (it != i_segment.indices.cend())
616       {
617         // if segment was found
618         cluster.indices.clear ();
619         cluster.indices.reserve (i_segment.indices.size ());
620         std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.indices));
621         break;
622       }
623     }// next segment
624   }// end if point was found
625 
626   deinitCompute ();
627 }
628 
629 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
630 template <typename PointT, typename NormalT> pcl::PointCloud<pcl::PointXYZRGB>::Ptr
getColoredCloud()631 pcl::RegionGrowing<PointT, NormalT>::getColoredCloud ()
632 {
633   pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud;
634 
635   if (!clusters_.empty ())
636   {
637     colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared ();
638 
639     srand (static_cast<unsigned int> (time (nullptr)));
640     std::vector<unsigned char> colors;
641     for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
642     {
643       colors.push_back (static_cast<unsigned char> (rand () % 256));
644       colors.push_back (static_cast<unsigned char> (rand () % 256));
645       colors.push_back (static_cast<unsigned char> (rand () % 256));
646     }
647 
648     colored_cloud->width = input_->width;
649     colored_cloud->height = input_->height;
650     colored_cloud->is_dense = input_->is_dense;
651     for (const auto& i_point: *input_)
652     {
653       pcl::PointXYZRGB point;
654       point.x = *(i_point.data);
655       point.y = *(i_point.data + 1);
656       point.z = *(i_point.data + 2);
657       point.r = 255;
658       point.g = 0;
659       point.b = 0;
660       colored_cloud->points.push_back (point);
661     }
662 
663     int next_color = 0;
664     for (const auto& i_segment : clusters_)
665     {
666       for (const auto& index : (i_segment.indices))
667       {
668         (*colored_cloud)[index].r = colors[3 * next_color];
669         (*colored_cloud)[index].g = colors[3 * next_color + 1];
670         (*colored_cloud)[index].b = colors[3 * next_color + 2];
671       }
672       next_color++;
673     }
674   }
675 
676   return (colored_cloud);
677 }
678 
679 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
680 template <typename PointT, typename NormalT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
getColoredCloudRGBA()681 pcl::RegionGrowing<PointT, NormalT>::getColoredCloudRGBA ()
682 {
683   pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud;
684 
685   if (!clusters_.empty ())
686   {
687     colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGBA>)->makeShared ();
688 
689     srand (static_cast<unsigned int> (time (nullptr)));
690     std::vector<unsigned char> colors;
691     for (std::size_t i_segment = 0; i_segment < clusters_.size (); i_segment++)
692     {
693       colors.push_back (static_cast<unsigned char> (rand () % 256));
694       colors.push_back (static_cast<unsigned char> (rand () % 256));
695       colors.push_back (static_cast<unsigned char> (rand () % 256));
696     }
697 
698     colored_cloud->width = input_->width;
699     colored_cloud->height = input_->height;
700     colored_cloud->is_dense = input_->is_dense;
701     for (const auto& i_point: *input_)
702     {
703       pcl::PointXYZRGBA point;
704       point.x = *(i_point.data);
705       point.y = *(i_point.data + 1);
706       point.z = *(i_point.data + 2);
707       point.r = 255;
708       point.g = 0;
709       point.b = 0;
710       point.a = 0;
711       colored_cloud->points.push_back (point);
712     }
713 
714     int next_color = 0;
715     for (const auto& i_segment : clusters_)
716     {
717       for (const auto& index : (i_segment.indices))
718       {
719         (*colored_cloud)[index].r = colors[3 * next_color];
720         (*colored_cloud)[index].g = colors[3 * next_color + 1];
721         (*colored_cloud)[index].b = colors[3 * next_color + 2];
722       }
723       next_color++;
724     }
725   }
726 
727   return (colored_cloud);
728 }
729 
730 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>;
731 
732