Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51template <typename PointT, typename NormalT>
53 point_distances_ (0),
54 segment_neighbours_ (0),
55 segment_distances_ (0),
56 segment_labels_ (0)
57{
58 normal_flag_ = false;
59 curvature_flag_ = false;
60 residual_flag_ = false;
62}
63
64//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
65template <typename PointT, typename NormalT>
67{
68 point_distances_.clear ();
69 segment_neighbours_.clear ();
70 segment_distances_.clear ();
71 segment_labels_.clear ();
72}
73
74//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75template <typename PointT, typename NormalT> float
77{
78 return (powf (color_p2p_threshold_, 0.5f));
79}
80
81//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82template <typename PointT, typename NormalT> void
84{
85 color_p2p_threshold_ = thresh * thresh;
86}
87
88//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
89template <typename PointT, typename NormalT> float
91{
92 return (powf (color_r2r_threshold_, 0.5f));
93}
94
95//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
96template <typename PointT, typename NormalT> void
98{
99 color_r2r_threshold_ = thresh * thresh;
100}
101
102//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
103template <typename PointT, typename NormalT> float
105{
106 return (powf (distance_threshold_, 0.5f));
107}
108
109//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
110template <typename PointT, typename NormalT> void
112{
113 distance_threshold_ = thresh * thresh;
114}
115
116//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
117template <typename PointT, typename NormalT> unsigned int
119{
120 return (region_neighbour_number_);
121}
122
123//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
124template <typename PointT, typename NormalT> void
126{
127 region_neighbour_number_ = nghbr_number;
128}
129
130//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
131template <typename PointT, typename NormalT> bool
133{
134 return (normal_flag_);
135}
136
137//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
138template <typename PointT, typename NormalT> void
140{
141 normal_flag_ = value;
142}
143
144//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
145template <typename PointT, typename NormalT> void
147{
148 curvature_flag_ = value;
149}
150
151//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
152template <typename PointT, typename NormalT> void
154{
155 residual_flag_ = value;
156}
157
158//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
159template <typename PointT, typename NormalT> void
160pcl::RegionGrowingRGB<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
161{
162 clusters_.clear ();
163 clusters.clear ();
164 point_neighbours_.clear ();
165 point_labels_.clear ();
166 num_pts_in_segment_.clear ();
167 point_distances_.clear ();
168 segment_neighbours_.clear ();
169 segment_distances_.clear ();
170 segment_labels_.clear ();
171 number_of_segments_ = 0;
172
173 bool segmentation_is_possible = initCompute ();
174 if ( !segmentation_is_possible )
175 {
176 deinitCompute ();
177 return;
178 }
179
180 segmentation_is_possible = prepareForSegmentation ();
181 if ( !segmentation_is_possible )
182 {
183 deinitCompute ();
184 return;
185 }
186
187 findPointNeighbours ();
188 applySmoothRegionGrowingAlgorithm ();
190
191 findSegmentNeighbours ();
192 applyRegionMergingAlgorithm ();
193
194 auto cluster_iter = clusters_.begin ();
195 while (cluster_iter != clusters_.end ())
196 {
197 if (cluster_iter->indices.size () < min_pts_per_cluster_ ||
198 cluster_iter->indices.size () > max_pts_per_cluster_)
199 {
200 cluster_iter = clusters_.erase (cluster_iter);
201 }
202 else
203 ++cluster_iter;
204 }
205
206 clusters.reserve (clusters_.size ());
207 std::copy (clusters_.cbegin (), clusters_.cend (), std::back_inserter (clusters));
208
209 deinitCompute ();
210}
211
212//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
213template <typename PointT, typename NormalT> bool
215{
216 // if user forgot to pass point cloud or if it is empty
217 if ( input_->points.empty () )
218 return (false);
219
220 // if normal/smoothness test is on then we need to check if all needed variables and parameters
221 // were correctly initialized
222 if (normal_flag_)
223 {
224 // if user forgot to pass normals or the sizes of point and normal cloud are different
225 if ( !normals_ || input_->size () != normals_->size () )
226 return (false);
227 }
228
229 // if residual test is on then we need to check if all needed parameters were correctly initialized
230 if (residual_flag_)
231 {
232 if (residual_threshold_ <= 0.0f)
233 return (false);
234 }
235
236 // if curvature test is on ...
237 // if (curvature_flag_)
238 // {
239 // in this case we do not need to check anything that related to it
240 // so we simply commented it
241 // }
242
243 // here we check the parameters related to color-based segmentation
244 if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f )
245 return (false);
246
247 // from here we check those parameters that are always valuable
248 if (neighbour_number_ == 0)
249 return (false);
250
251 // if user didn't set search method
252 if (!search_)
253 search_.reset (new pcl::search::KdTree<PointT>);
254
255 if (indices_)
256 {
257 if (indices_->empty ())
258 PCL_ERROR ("[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n");
259 search_->setInputCloud (input_, indices_);
260 }
261 else
262 search_->setInputCloud (input_);
263
264 return (true);
265}
266
267//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
268template <typename PointT, typename NormalT> void
270{
271 pcl::Indices neighbours;
272 std::vector<float> distances;
273
274 point_neighbours_.resize (input_->size (), neighbours);
275 point_distances_.resize (input_->size (), distances);
276
277 for (const auto& point_index: (*indices_))
278 {
279 neighbours.clear ();
280 distances.clear ();
281 search_->nearestKSearch (point_index, region_neighbour_number_, neighbours, distances);
282 point_neighbours_[point_index].swap (neighbours);
283 point_distances_[point_index].swap (distances);
284 }
285}
286
287//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
288template <typename PointT, typename NormalT> void
290{
291 pcl::Indices neighbours;
292 std::vector<float> distances;
293 segment_neighbours_.resize (number_of_segments_, neighbours);
294 segment_distances_.resize (number_of_segments_, distances);
295
296 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
297 {
298 pcl::Indices nghbrs;
299 std::vector<float> dist;
300 findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist);
301 segment_neighbours_[i_seg].swap (nghbrs);
302 segment_distances_[i_seg].swap (dist);
303 }
304}
305
306//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
307template <typename PointT,typename NormalT> void
309{
310 std::vector<float> distances;
311 float max_dist = std::numeric_limits<float>::max ();
312 distances.resize (clusters_.size (), max_dist);
313
314 const auto number_of_points = num_pts_in_segment_[index];
315 //loop through every point in this segment and check neighbours
316 for (pcl::uindex_t i_point = 0; i_point < number_of_points; i_point++)
317 {
318 const auto point_index = clusters_[index].indices[i_point];
319 const auto number_of_neighbours = point_neighbours_[point_index].size ();
320 //loop through every neighbour of the current point, find out to which segment it belongs
321 //and if it belongs to neighbouring segment and is close enough then remember segment and its distance
322 for (std::size_t i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++)
323 {
324 // find segment
325 const pcl::index_t segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ];
326
327 if ( segment_index != index )
328 {
329 // try to push it to the queue
330 if (distances[segment_index] > point_distances_[point_index][i_nghbr])
331 distances[segment_index] = point_distances_[point_index][i_nghbr];
332 }
333 }
334 }// next point
335
336 std::priority_queue<std::pair<float, int> > segment_neighbours;
337 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
338 {
339 if (distances[i_seg] < max_dist)
340 {
341 segment_neighbours.emplace (distances[i_seg], i_seg);
342 if (segment_neighbours.size () > nghbr_number)
343 segment_neighbours.pop ();
344 }
345 }
346
347 const std::size_t size = std::min<std::size_t> (segment_neighbours.size (), static_cast<std::size_t>(nghbr_number));
348 nghbrs.resize (size, 0);
349 dist.resize (size, 0);
350 pcl::uindex_t counter = 0;
351 while ( !segment_neighbours.empty () && counter < nghbr_number )
352 {
353 dist[counter] = segment_neighbours.top ().first;
354 nghbrs[counter] = segment_neighbours.top ().second;
355 segment_neighbours.pop ();
356 counter++;
357 }
358}
359
360//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
361template <typename PointT, typename NormalT> void
363{
364 // calculate color of each segment
365 std::vector< std::vector<unsigned int> > segment_color;
366 std::vector<unsigned int> color;
367 color.resize (3, 0);
368 segment_color.resize (number_of_segments_, color);
369
370 for (const auto& point_index : (*indices_))
371 {
372 int segment_index = point_labels_[point_index];
373 segment_color[segment_index][0] += (*input_)[point_index].r;
374 segment_color[segment_index][1] += (*input_)[point_index].g;
375 segment_color[segment_index][2] += (*input_)[point_index].b;
376 }
377 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
378 {
379 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]));
380 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]));
381 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]));
382 }
383
384 // now it is time to find out if there are segments with a similar color
385 // and merge them together
386 std::vector<unsigned int> num_pts_in_homogeneous_region;
387 std::vector<int> num_seg_in_homogeneous_region;
388
389 segment_labels_.resize (number_of_segments_, -1);
390
391 float dist_thresh = distance_threshold_;
392 int homogeneous_region_number = 0;
393 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
394 {
395 int curr_homogeneous_region = 0;
396 if (segment_labels_[i_seg] == -1)
397 {
398 segment_labels_[i_seg] = homogeneous_region_number;
399 curr_homogeneous_region = homogeneous_region_number;
400 num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]);
401 num_seg_in_homogeneous_region.push_back (1);
402 homogeneous_region_number++;
403 }
404 else
405 curr_homogeneous_region = segment_labels_[i_seg];
406
407 unsigned int i_nghbr = 0;
408 while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () )
409 {
410 int index = segment_neighbours_[i_seg][i_nghbr];
411 if (segment_distances_[i_seg][i_nghbr] > dist_thresh)
412 {
413 i_nghbr++;
414 continue;
415 }
416 if ( segment_labels_[index] == -1 )
417 {
418 float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]);
419 if (difference < color_r2r_threshold_)
420 {
421 segment_labels_[index] = curr_homogeneous_region;
422 num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index];
423 num_seg_in_homogeneous_region[curr_homogeneous_region] += 1;
424 }
425 }
426 i_nghbr++;
427 }// next neighbour
428 }// next segment
429
430 segment_color.clear ();
431 color.clear ();
432
433 std::vector< std::vector<int> > final_segments;
434 std::vector<int> region;
435 final_segments.resize (homogeneous_region_number, region);
436 for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
437 {
438 final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0);
439 }
440
441 std::vector<int> counter;
442 counter.resize (homogeneous_region_number, 0);
443 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++)
444 {
445 int index = segment_labels_[i_seg];
446 final_segments[ index ][ counter[index] ] = i_seg;
447 counter[index] += 1;
448 }
449
450 std::vector< std::vector< std::pair<float, pcl::index_t> > > region_neighbours;
451 findRegionNeighbours (region_neighbours, final_segments);
452
453 int final_segment_number = homogeneous_region_number;
454 for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++)
455 {
456 if (num_pts_in_homogeneous_region[i_reg] < min_pts_per_cluster_)
457 {
458 if ( region_neighbours[i_reg].empty () )
459 continue;
460 int nearest_neighbour = region_neighbours[i_reg][0].second;
461 if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () )
462 continue;
463 int reg_index = segment_labels_[nearest_neighbour];
464 int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg];
465 for (int i_seg = 0; i_seg < num_seg_in_reg; i_seg++)
466 {
467 int segment_index = final_segments[i_reg][i_seg];
468 final_segments[reg_index].push_back (segment_index);
469 segment_labels_[segment_index] = reg_index;
470 }
471 final_segments[i_reg].clear ();
472 num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg];
473 num_pts_in_homogeneous_region[i_reg] = 0;
474 num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg];
475 num_seg_in_homogeneous_region[i_reg] = 0;
476 final_segment_number -= 1;
477
478 for (auto& nghbr : region_neighbours[reg_index])
479 {
480 if ( segment_labels_[ nghbr.second ] == reg_index )
481 {
482 nghbr.first = std::numeric_limits<float>::max ();
483 nghbr.second = 0;
484 }
485 }
486 for (const auto& nghbr : region_neighbours[i_reg])
487 {
488 if ( segment_labels_[ nghbr.second ] != reg_index )
489 {
490 region_neighbours[reg_index].push_back (nghbr);
491 }
492 }
493 region_neighbours[i_reg].clear ();
494 std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (), comparePair);
495 }
496 }
497
498 assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ()));
499
500 number_of_segments_ = final_segment_number;
501}
502
503//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
504template <typename PointT, typename NormalT> float
505pcl::RegionGrowingRGB<PointT, NormalT>::calculateColorimetricalDifference (std::vector<unsigned int>& first_color, std::vector<unsigned int>& second_color) const
506{
507 float difference = 0.0f;
508 difference += static_cast<float>((first_color[0] - second_color[0]) * (first_color[0] - second_color[0]));
509 difference += static_cast<float>((first_color[1] - second_color[1]) * (first_color[1] - second_color[1]));
510 difference += static_cast<float>((first_color[2] - second_color[2]) * (first_color[2] - second_color[2]));
511 return (difference);
512}
513
514//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
515template <typename PointT, typename NormalT> void
516pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::vector< std::pair<float, pcl::index_t> > >& neighbours_out, std::vector< std::vector<int> >& regions_in)
517{
518 int region_number = static_cast<int> (regions_in.size ());
519 neighbours_out.clear ();
520 neighbours_out.resize (region_number);
521
522 for (int i_reg = 0; i_reg < region_number; i_reg++)
523 {
524 neighbours_out[i_reg].reserve (regions_in[i_reg].size () * region_neighbour_number_);
525 for (const auto& curr_segment : regions_in[i_reg])
526 {
527 const std::size_t nghbr_number = segment_neighbours_[curr_segment].size ();
528 std::pair<float, pcl::index_t> pair;
529 for (std::size_t i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++)
530 {
531 const auto segment_index = segment_neighbours_[curr_segment][i_nghbr];
532 if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () )
533 continue;
534 if (segment_labels_[segment_index] != i_reg)
535 {
536 pair.first = segment_distances_[curr_segment][i_nghbr];
537 pair.second = segment_index;
538 neighbours_out[i_reg].push_back (pair);
539 }
540 }// next neighbour
541 }// next segment
542 std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (), comparePair);
543 }// next homogeneous region
544}
545
546//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
547template <typename PointT, typename NormalT> void
548pcl::RegionGrowingRGB<PointT, NormalT>::assembleRegions (std::vector<unsigned int>& num_pts_in_region, int num_regions)
549{
550 clusters_.clear ();
551 pcl::PointIndices segment;
552 clusters_.resize (num_regions, segment);
553 for (int i_seg = 0; i_seg < num_regions; i_seg++)
554 {
555 clusters_[i_seg].indices.resize (num_pts_in_region[i_seg]);
556 }
557
558 std::vector<int> counter;
559 counter.resize (num_regions, 0);
560 for (const auto& point_index : (*indices_))
561 {
562 int index = point_labels_[point_index];
563 index = segment_labels_[index];
564 clusters_[index].indices[ counter[index] ] = point_index;
565 counter[index] += 1;
566 }
567
568 // now we need to erase empty regions
569 if (clusters_.empty ())
570 return;
571
572 std::vector<pcl::PointIndices>::iterator itr1, itr2;
573 itr1 = clusters_.begin ();
574 itr2 = clusters_.end () - 1;
575
576 while (itr1 < itr2)
577 {
578 while (!(itr1->indices.empty ()) && itr1 < itr2)
579 ++itr1;
580 while ( itr2->indices.empty () && itr1 < itr2)
581 --itr2;
582
583 if (itr1 != itr2)
584 itr1->indices.swap (itr2->indices);
585 }
586
587 if (itr2->indices.empty ())
588 clusters_.erase (itr2, clusters_.end ());
589}
590
591//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
592template <typename PointT, typename NormalT> bool
594{
595 is_a_seed = true;
596
597 // check the color difference
598 std::vector<unsigned int> point_color;
599 point_color.resize (3, 0);
600 std::vector<unsigned int> nghbr_color;
601 nghbr_color.resize (3, 0);
602 point_color[0] = (*input_)[point].r;
603 point_color[1] = (*input_)[point].g;
604 point_color[2] = (*input_)[point].b;
605 nghbr_color[0] = (*input_)[nghbr].r;
606 nghbr_color[1] = (*input_)[nghbr].g;
607 nghbr_color[2] = (*input_)[nghbr].b;
608 float difference = calculateColorimetricalDifference (point_color, nghbr_color);
609 if (difference > color_p2p_threshold_)
610 return (false);
611
612 float cosine_threshold = std::cos (theta_threshold_);
613
614 // check the angle between normals if needed
615 if (normal_flag_)
616 {
617 float data[4];
618 data[0] = (*input_)[point].data[0];
619 data[1] = (*input_)[point].data[1];
620 data[2] = (*input_)[point].data[2];
621 data[3] = (*input_)[point].data[3];
622
623 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data));
624 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
625 if (smooth_mode_flag_ == true)
626 {
627 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
628 float dot_product = std::abs (nghbr_normal.dot (initial_normal));
629 if (dot_product < cosine_threshold)
630 return (false);
631 }
632 else
633 {
634 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> ((*normals_)[nghbr].normal));
635 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> ((*normals_)[initial_seed].normal));
636 float dot_product = std::abs (nghbr_normal.dot (initial_seed_normal));
637 if (dot_product < cosine_threshold)
638 return (false);
639 }
640 }
641
642 // check the curvature if needed
643 if (curvature_flag_ && (*normals_)[nghbr].curvature > curvature_threshold_)
644 is_a_seed = false;
645
646 // check the residual if needed
647 if (residual_flag_)
648 {
649 float data_p[4];
650 data_p[0] = (*input_)[point].data[0];
651 data_p[1] = (*input_)[point].data[1];
652 data_p[2] = (*input_)[point].data[2];
653 data_p[3] = (*input_)[point].data[3];
654 float data_n[4];
655 data_n[0] = (*input_)[nghbr].data[0];
656 data_n[1] = (*input_)[nghbr].data[1];
657 data_n[2] = (*input_)[nghbr].data[2];
658 data_n[3] = (*input_)[nghbr].data[3];
659 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n));
660 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p));
661 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> ((*normals_)[point].normal));
662 float residual = std::abs (initial_normal.dot (initial_point - nghbr_point));
663 if (residual > residual_threshold_)
664 is_a_seed = false;
665 }
666
667 return (true);
668}
669
670//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
671template <typename PointT, typename NormalT> void
673{
674 cluster.indices.clear ();
675
676 bool segmentation_is_possible = initCompute ();
677 if ( !segmentation_is_possible )
678 {
679 deinitCompute ();
680 return;
681 }
682
683 // first of all we need to find out if this point belongs to cloud
684 bool point_was_found = false;
685 for (const auto& point : (*indices_))
686 if (point == index)
687 {
688 point_was_found = true;
689 break;
690 }
691
692 if (point_was_found)
693 {
694 if (clusters_.empty ())
695 {
696 clusters_.clear ();
697 point_neighbours_.clear ();
698 point_labels_.clear ();
699 num_pts_in_segment_.clear ();
700 point_distances_.clear ();
701 segment_neighbours_.clear ();
702 segment_distances_.clear ();
703 segment_labels_.clear ();
704 number_of_segments_ = 0;
705
706 segmentation_is_possible = prepareForSegmentation ();
707 if ( !segmentation_is_possible )
708 {
709 deinitCompute ();
710 return;
711 }
712
713 findPointNeighbours ();
714 applySmoothRegionGrowingAlgorithm ();
716
717 findSegmentNeighbours ();
718 applyRegionMergingAlgorithm ();
719 }
720 // if we have already made the segmentation, then find the segment
721 // to which this point belongs
722 for (const auto& i_segment : clusters_)
723 {
724 const auto it = std::find (i_segment.indices.cbegin (), i_segment.indices.cend (), index);
725 if (it != i_segment.indices.cend())
726 {
727 // if segment was found
728 cluster.indices.clear ();
729 cluster.indices.reserve (i_segment.indices.size ());
730 std::copy (i_segment.indices.begin (), i_segment.indices.end (), std::back_inserter (cluster.indices));
731 break;
732 }
733 }// next segment
734 }// end if point was found
735
736 deinitCompute ();
737}
738
739#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