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