Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the copyright holder(s) nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author : Sergey Ushakov 00036 * Email : mine_all_mine@bk.ru 00037 * 00038 */ 00039 00040 #ifndef PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_ 00041 #define PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_ 00042 00043 #include <pcl/segmentation/region_growing_rgb.h> 00044 #include <pcl/search/search.h> 00045 #include <pcl/search/kdtree.h> 00046 00047 #include <queue> 00048 00049 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00050 template <typename PointT, typename NormalT> 00051 pcl::RegionGrowingRGB<PointT, NormalT>::RegionGrowingRGB () : 00052 color_p2p_threshold_ (1225.0f), 00053 color_r2r_threshold_ (10.0f), 00054 distance_threshold_ (0.05f), 00055 region_neighbour_number_ (100), 00056 point_distances_ (0), 00057 segment_neighbours_ (0), 00058 segment_distances_ (0), 00059 segment_labels_ (0) 00060 { 00061 normal_flag_ = false; 00062 curvature_flag_ = false; 00063 residual_flag_ = false; 00064 min_pts_per_cluster_ = 10; 00065 } 00066 00067 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00068 template <typename PointT, typename NormalT> 00069 pcl::RegionGrowingRGB<PointT, NormalT>::~RegionGrowingRGB () 00070 { 00071 point_distances_.clear (); 00072 segment_neighbours_.clear (); 00073 segment_distances_.clear (); 00074 segment_labels_.clear (); 00075 } 00076 00077 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00078 template <typename PointT, typename NormalT> float 00079 pcl::RegionGrowingRGB<PointT, NormalT>::getPointColorThreshold () const 00080 { 00081 return (powf (color_p2p_threshold_, 0.5f)); 00082 } 00083 00084 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00085 template <typename PointT, typename NormalT> void 00086 pcl::RegionGrowingRGB<PointT, NormalT>::setPointColorThreshold (float thresh) 00087 { 00088 color_p2p_threshold_ = thresh * thresh; 00089 } 00090 00091 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00092 template <typename PointT, typename NormalT> float 00093 pcl::RegionGrowingRGB<PointT, NormalT>::getRegionColorThreshold () const 00094 { 00095 return (powf (color_r2r_threshold_, 0.5f)); 00096 } 00097 00098 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00099 template <typename PointT, typename NormalT> void 00100 pcl::RegionGrowingRGB<PointT, NormalT>::setRegionColorThreshold (float thresh) 00101 { 00102 color_r2r_threshold_ = thresh * thresh; 00103 } 00104 00105 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00106 template <typename PointT, typename NormalT> float 00107 pcl::RegionGrowingRGB<PointT, NormalT>::getDistanceThreshold () const 00108 { 00109 return (powf (distance_threshold_, 0.5f)); 00110 } 00111 00112 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00113 template <typename PointT, typename NormalT> void 00114 pcl::RegionGrowingRGB<PointT, NormalT>::setDistanceThreshold (float thresh) 00115 { 00116 distance_threshold_ = thresh * thresh; 00117 } 00118 00119 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00120 template <typename PointT, typename NormalT> unsigned int 00121 pcl::RegionGrowingRGB<PointT, NormalT>::getNumberOfRegionNeighbours () const 00122 { 00123 return (region_neighbour_number_); 00124 } 00125 00126 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00127 template <typename PointT, typename NormalT> void 00128 pcl::RegionGrowingRGB<PointT, NormalT>::setNumberOfRegionNeighbours (unsigned int nghbr_number) 00129 { 00130 region_neighbour_number_ = nghbr_number; 00131 } 00132 00133 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00134 template <typename PointT, typename NormalT> bool 00135 pcl::RegionGrowingRGB<PointT, NormalT>::getNormalTestFlag () const 00136 { 00137 return (normal_flag_); 00138 } 00139 00140 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00141 template <typename PointT, typename NormalT> void 00142 pcl::RegionGrowingRGB<PointT, NormalT>::setNormalTestFlag (bool value) 00143 { 00144 normal_flag_ = value; 00145 } 00146 00147 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00148 template <typename PointT, typename NormalT> void 00149 pcl::RegionGrowingRGB<PointT, NormalT>::setCurvatureTestFlag (bool value) 00150 { 00151 curvature_flag_ = value; 00152 } 00153 00154 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00155 template <typename PointT, typename NormalT> void 00156 pcl::RegionGrowingRGB<PointT, NormalT>::setResidualTestFlag (bool value) 00157 { 00158 residual_flag_ = value; 00159 } 00160 00161 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00162 template <typename PointT, typename NormalT> void 00163 pcl::RegionGrowingRGB<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters) 00164 { 00165 clusters_.clear (); 00166 clusters.clear (); 00167 point_neighbours_.clear (); 00168 point_labels_.clear (); 00169 num_pts_in_segment_.clear (); 00170 point_distances_.clear (); 00171 segment_neighbours_.clear (); 00172 segment_distances_.clear (); 00173 segment_labels_.clear (); 00174 number_of_segments_ = 0; 00175 00176 bool segmentation_is_possible = initCompute (); 00177 if ( !segmentation_is_possible ) 00178 { 00179 deinitCompute (); 00180 return; 00181 } 00182 00183 segmentation_is_possible = prepareForSegmentation (); 00184 if ( !segmentation_is_possible ) 00185 { 00186 deinitCompute (); 00187 return; 00188 } 00189 00190 findPointNeighbours (); 00191 applySmoothRegionGrowingAlgorithm (); 00192 RegionGrowing<PointT, NormalT>::assembleRegions (); 00193 00194 findSegmentNeighbours (); 00195 applyRegionMergingAlgorithm (); 00196 00197 std::vector<pcl::PointIndices>::iterator cluster_iter = clusters_.begin (); 00198 while (cluster_iter != clusters_.end ()) 00199 { 00200 if (cluster_iter->indices.size () < min_pts_per_cluster_ || cluster_iter->indices.size () > max_pts_per_cluster_) 00201 { 00202 cluster_iter = clusters_.erase (cluster_iter); 00203 } 00204 else 00205 cluster_iter++; 00206 } 00207 00208 clusters.reserve (clusters_.size ()); 00209 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters)); 00210 00211 deinitCompute (); 00212 } 00213 00214 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00215 template <typename PointT, typename NormalT> bool 00216 pcl::RegionGrowingRGB<PointT, NormalT>::prepareForSegmentation () 00217 { 00218 // if user forgot to pass point cloud or if it is empty 00219 if ( input_->points.size () == 0 ) 00220 return (false); 00221 00222 // if normal/smoothness test is on then we need to check if all needed variables and parameters 00223 // were correctly initialized 00224 if (normal_flag_) 00225 { 00226 // if user forgot to pass normals or the sizes of point and normal cloud are different 00227 if ( normals_ == 0 || input_->points.size () != normals_->points.size () ) 00228 return (false); 00229 } 00230 00231 // if residual test is on then we need to check if all needed parameters were correctly initialized 00232 if (residual_flag_) 00233 { 00234 if (residual_threshold_ <= 0.0f) 00235 return (false); 00236 } 00237 00238 // if curvature test is on ... 00239 // if (curvature_flag_) 00240 // { 00241 // in this case we do not need to check anything that related to it 00242 // so we simply commented it 00243 // } 00244 00245 // here we check the parameters related to color-based segmentation 00246 if ( region_neighbour_number_ == 0 || color_p2p_threshold_ < 0.0f || color_r2r_threshold_ < 0.0f || distance_threshold_ < 0.0f ) 00247 return (false); 00248 00249 // from here we check those parameters that are always valuable 00250 if (neighbour_number_ == 0) 00251 return (false); 00252 00253 // if user didn't set search method 00254 if (!search_) 00255 search_.reset (new pcl::search::KdTree<PointT>); 00256 00257 if (indices_) 00258 { 00259 if (indices_->empty ()) 00260 PCL_ERROR ("[pcl::RegionGrowingRGB::prepareForSegmentation] Empty given indices!\n"); 00261 search_->setInputCloud (input_, indices_); 00262 } 00263 else 00264 search_->setInputCloud (input_); 00265 00266 return (true); 00267 } 00268 00269 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00270 template <typename PointT, typename NormalT> void 00271 pcl::RegionGrowingRGB<PointT, NormalT>::findPointNeighbours () 00272 { 00273 int point_number = static_cast<int> (indices_->size ()); 00274 std::vector<int> neighbours; 00275 std::vector<float> distances; 00276 00277 point_neighbours_.resize (input_->points.size (), neighbours); 00278 point_distances_.resize (input_->points.size (), distances); 00279 00280 for (int i_point = 0; i_point < point_number; i_point++) 00281 { 00282 int point_index = (*indices_)[i_point]; 00283 neighbours.clear (); 00284 distances.clear (); 00285 search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances); 00286 point_neighbours_[point_index].swap (neighbours); 00287 point_distances_[point_index].swap (distances); 00288 } 00289 } 00290 00291 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00292 template <typename PointT, typename NormalT> void 00293 pcl::RegionGrowingRGB<PointT, NormalT>::findSegmentNeighbours () 00294 { 00295 std::vector<int> neighbours; 00296 std::vector<float> distances; 00297 segment_neighbours_.resize (number_of_segments_, neighbours); 00298 segment_distances_.resize (number_of_segments_, distances); 00299 00300 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) 00301 { 00302 std::vector<int> nghbrs; 00303 std::vector<float> dist; 00304 findRegionsKNN (i_seg, region_neighbour_number_, nghbrs, dist); 00305 segment_neighbours_[i_seg].swap (nghbrs); 00306 segment_distances_[i_seg].swap (dist); 00307 } 00308 } 00309 00310 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00311 template <typename PointT,typename NormalT> void 00312 pcl::RegionGrowingRGB<PointT, NormalT>::findRegionsKNN (int index, int nghbr_number, std::vector<int>& nghbrs, std::vector<float>& dist) 00313 { 00314 std::vector<float> distances; 00315 float max_dist = std::numeric_limits<float>::max (); 00316 distances.resize (clusters_.size (), max_dist); 00317 00318 int number_of_points = num_pts_in_segment_[index]; 00319 //loop throug every point in this segment and check neighbours 00320 for (int i_point = 0; i_point < number_of_points; i_point++) 00321 { 00322 int point_index = clusters_[index].indices[i_point]; 00323 int number_of_neighbours = static_cast<int> (point_neighbours_[point_index].size ()); 00324 //loop throug every neighbour of the current point, find out to which segment it belongs 00325 //and if it belongs to neighbouring segment and is close enough then remember segment and its distance 00326 for (int i_nghbr = 0; i_nghbr < number_of_neighbours; i_nghbr++) 00327 { 00328 // find segment 00329 int segment_index = -1; 00330 segment_index = point_labels_[ point_neighbours_[point_index][i_nghbr] ]; 00331 00332 if ( segment_index != index ) 00333 { 00334 // try to push it to the queue 00335 if (distances[segment_index] > point_distances_[point_index][i_nghbr]) 00336 distances[segment_index] = point_distances_[point_index][i_nghbr]; 00337 } 00338 } 00339 }// next point 00340 00341 std::priority_queue<std::pair<float, int> > segment_neighbours; 00342 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) 00343 { 00344 if (distances[i_seg] < max_dist) 00345 { 00346 segment_neighbours.push (std::make_pair (distances[i_seg], i_seg) ); 00347 if (int (segment_neighbours.size ()) > nghbr_number) 00348 segment_neighbours.pop (); 00349 } 00350 } 00351 00352 int size = std::min<int> (static_cast<int> (segment_neighbours.size ()), nghbr_number); 00353 nghbrs.resize (size, 0); 00354 dist.resize (size, 0); 00355 int counter = 0; 00356 while ( !segment_neighbours.empty () && counter < nghbr_number ) 00357 { 00358 dist[counter] = segment_neighbours.top ().first; 00359 nghbrs[counter] = segment_neighbours.top ().second; 00360 segment_neighbours.pop (); 00361 counter++; 00362 } 00363 } 00364 00365 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00366 template <typename PointT, typename NormalT> void 00367 pcl::RegionGrowingRGB<PointT, NormalT>::applyRegionMergingAlgorithm () 00368 { 00369 int number_of_points = static_cast<int> (indices_->size ()); 00370 00371 // calculate color of each segment 00372 std::vector< std::vector<unsigned int> > segment_color; 00373 std::vector<unsigned int> color; 00374 color.resize (3, 0); 00375 segment_color.resize (number_of_segments_, color); 00376 00377 for (int i_point = 0; i_point < number_of_points; i_point++) 00378 { 00379 int point_index = (*indices_)[i_point]; 00380 int segment_index = point_labels_[point_index]; 00381 segment_color[segment_index][0] += input_->points[point_index].r; 00382 segment_color[segment_index][1] += input_->points[point_index].g; 00383 segment_color[segment_index][2] += input_->points[point_index].b; 00384 } 00385 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) 00386 { 00387 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])); 00388 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])); 00389 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])); 00390 } 00391 00392 // now it is time to find out if there are segments with a similar color 00393 // and merge them together 00394 std::vector<unsigned int> num_pts_in_homogeneous_region; 00395 std::vector<int> num_seg_in_homogeneous_region; 00396 00397 segment_labels_.resize (number_of_segments_, -1); 00398 00399 float dist_thresh = distance_threshold_; 00400 int homogeneous_region_number = 0; 00401 int curr_homogeneous_region = 0; 00402 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) 00403 { 00404 curr_homogeneous_region = 0; 00405 if (segment_labels_[i_seg] == -1) 00406 { 00407 segment_labels_[i_seg] = homogeneous_region_number; 00408 curr_homogeneous_region = homogeneous_region_number; 00409 num_pts_in_homogeneous_region.push_back (num_pts_in_segment_[i_seg]); 00410 num_seg_in_homogeneous_region.push_back (1); 00411 homogeneous_region_number++; 00412 } 00413 else 00414 curr_homogeneous_region = segment_labels_[i_seg]; 00415 00416 unsigned int i_nghbr = 0; 00417 while ( i_nghbr < region_neighbour_number_ && i_nghbr < segment_neighbours_[i_seg].size () ) 00418 { 00419 int index = segment_neighbours_[i_seg][i_nghbr]; 00420 if (segment_distances_[i_seg][i_nghbr] > dist_thresh) 00421 { 00422 i_nghbr++; 00423 continue; 00424 } 00425 if ( segment_labels_[index] == -1 ) 00426 { 00427 float difference = calculateColorimetricalDifference (segment_color[i_seg], segment_color[index]); 00428 if (difference < color_r2r_threshold_) 00429 { 00430 segment_labels_[index] = curr_homogeneous_region; 00431 num_pts_in_homogeneous_region[curr_homogeneous_region] += num_pts_in_segment_[index]; 00432 num_seg_in_homogeneous_region[curr_homogeneous_region] += 1; 00433 } 00434 } 00435 i_nghbr++; 00436 }// next neighbour 00437 }// next segment 00438 00439 segment_color.clear (); 00440 color.clear (); 00441 00442 std::vector< std::vector<int> > final_segments; 00443 std::vector<int> region; 00444 final_segments.resize (homogeneous_region_number, region); 00445 for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++) 00446 { 00447 final_segments[i_reg].resize (num_seg_in_homogeneous_region[i_reg], 0); 00448 } 00449 00450 std::vector<int> counter; 00451 counter.resize (homogeneous_region_number, 0); 00452 for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) 00453 { 00454 int index = segment_labels_[i_seg]; 00455 final_segments[ index ][ counter[index] ] = i_seg; 00456 counter[index] += 1; 00457 } 00458 00459 std::vector< std::vector< std::pair<float, int> > > region_neighbours; 00460 findRegionNeighbours (region_neighbours, final_segments); 00461 00462 int final_segment_number = homogeneous_region_number; 00463 for (int i_reg = 0; i_reg < homogeneous_region_number; i_reg++) 00464 { 00465 if (num_pts_in_homogeneous_region[i_reg] < min_pts_per_cluster_) 00466 { 00467 if ( region_neighbours[i_reg].empty () ) 00468 continue; 00469 int nearest_neighbour = region_neighbours[i_reg][0].second; 00470 if ( region_neighbours[i_reg][0].first == std::numeric_limits<float>::max () ) 00471 continue; 00472 int reg_index = segment_labels_[nearest_neighbour]; 00473 int num_seg_in_reg = num_seg_in_homogeneous_region[i_reg]; 00474 for (int i_seg = 0; i_seg < num_seg_in_reg; i_seg++) 00475 { 00476 int segment_index = final_segments[i_reg][i_seg]; 00477 final_segments[reg_index].push_back (segment_index); 00478 segment_labels_[segment_index] = reg_index; 00479 } 00480 final_segments[i_reg].clear (); 00481 num_pts_in_homogeneous_region[reg_index] += num_pts_in_homogeneous_region[i_reg]; 00482 num_pts_in_homogeneous_region[i_reg] = 0; 00483 num_seg_in_homogeneous_region[reg_index] += num_seg_in_homogeneous_region[i_reg]; 00484 num_seg_in_homogeneous_region[i_reg] = 0; 00485 final_segment_number -= 1; 00486 00487 int nghbr_number = static_cast<int> (region_neighbours[reg_index].size ()); 00488 for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++) 00489 { 00490 if ( segment_labels_[ region_neighbours[reg_index][i_nghbr].second ] == reg_index ) 00491 { 00492 region_neighbours[reg_index][i_nghbr].first = std::numeric_limits<float>::max (); 00493 region_neighbours[reg_index][i_nghbr].second = 0; 00494 } 00495 } 00496 nghbr_number = static_cast<int> (region_neighbours[i_reg].size ()); 00497 for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++) 00498 { 00499 if ( segment_labels_[ region_neighbours[i_reg][i_nghbr].second ] != reg_index ) 00500 { 00501 std::pair<float, int> pair; 00502 pair.first = region_neighbours[i_reg][i_nghbr].first; 00503 pair.second = region_neighbours[i_reg][i_nghbr].second; 00504 region_neighbours[reg_index].push_back (pair); 00505 } 00506 } 00507 region_neighbours[i_reg].clear (); 00508 std::sort (region_neighbours[reg_index].begin (), region_neighbours[reg_index].end (), comparePair); 00509 } 00510 } 00511 00512 assembleRegions (num_pts_in_homogeneous_region, static_cast<int> (num_pts_in_homogeneous_region.size ())); 00513 00514 number_of_segments_ = final_segment_number; 00515 } 00516 00517 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00518 template <typename PointT, typename NormalT> float 00519 pcl::RegionGrowingRGB<PointT, NormalT>::calculateColorimetricalDifference (std::vector<unsigned int>& first_color, std::vector<unsigned int>& second_color) const 00520 { 00521 float difference = 0.0f; 00522 difference += float ((first_color[0] - second_color[0]) * (first_color[0] - second_color[0])); 00523 difference += float ((first_color[1] - second_color[1]) * (first_color[1] - second_color[1])); 00524 difference += float ((first_color[2] - second_color[2]) * (first_color[2] - second_color[2])); 00525 return (difference); 00526 } 00527 00528 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00529 template <typename PointT, typename NormalT> void 00530 pcl::RegionGrowingRGB<PointT, NormalT>::findRegionNeighbours (std::vector< std::vector< std::pair<float, int> > >& neighbours_out, std::vector< std::vector<int> >& regions_in) 00531 { 00532 int region_number = static_cast<int> (regions_in.size ()); 00533 neighbours_out.clear (); 00534 neighbours_out.resize (region_number); 00535 00536 for (int i_reg = 0; i_reg < region_number; i_reg++) 00537 { 00538 int segment_num = static_cast<int> (regions_in[i_reg].size ()); 00539 neighbours_out[i_reg].reserve (segment_num * region_neighbour_number_); 00540 for (int i_seg = 0; i_seg < segment_num; i_seg++) 00541 { 00542 int curr_segment = regions_in[i_reg][i_seg]; 00543 int nghbr_number = static_cast<int> (segment_neighbours_[curr_segment].size ()); 00544 std::pair<float, int> pair; 00545 for (int i_nghbr = 0; i_nghbr < nghbr_number; i_nghbr++) 00546 { 00547 int segment_index = segment_neighbours_[curr_segment][i_nghbr]; 00548 if ( segment_distances_[curr_segment][i_nghbr] == std::numeric_limits<float>::max () ) 00549 continue; 00550 if (segment_labels_[segment_index] != i_reg) 00551 { 00552 pair.first = segment_distances_[curr_segment][i_nghbr]; 00553 pair.second = segment_index; 00554 neighbours_out[i_reg].push_back (pair); 00555 } 00556 }// next neighbour 00557 }// next segment 00558 std::sort (neighbours_out[i_reg].begin (), neighbours_out[i_reg].end (), comparePair); 00559 }// next homogeneous region 00560 } 00561 00562 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00563 template <typename PointT, typename NormalT> void 00564 pcl::RegionGrowingRGB<PointT, NormalT>::assembleRegions (std::vector<unsigned int>& num_pts_in_region, int num_regions) 00565 { 00566 clusters_.clear (); 00567 pcl::PointIndices segment; 00568 clusters_.resize (num_regions, segment); 00569 for (int i_seg = 0; i_seg < num_regions; i_seg++) 00570 { 00571 clusters_[i_seg].indices.resize (num_pts_in_region[i_seg]); 00572 } 00573 00574 std::vector<int> counter; 00575 counter.resize (num_regions, 0); 00576 int point_number = static_cast<int> (indices_->size ()); 00577 for (int i_point = 0; i_point < point_number; i_point++) 00578 { 00579 int point_index = (*indices_)[i_point]; 00580 int index = point_labels_[point_index]; 00581 index = segment_labels_[index]; 00582 clusters_[index].indices[ counter[index] ] = point_index; 00583 counter[index] += 1; 00584 } 00585 00586 // now we need to erase empty regions 00587 std::vector< pcl::PointIndices >::iterator i_region; 00588 i_region = clusters_.begin (); 00589 while(i_region != clusters_.end ()) 00590 { 00591 if ( i_region->indices.empty () ) 00592 i_region = clusters_.erase (i_region); 00593 else 00594 i_region++; 00595 } 00596 } 00597 00598 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00599 template <typename PointT, typename NormalT> bool 00600 pcl::RegionGrowingRGB<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const 00601 { 00602 is_a_seed = true; 00603 00604 // check the color difference 00605 std::vector<unsigned int> point_color; 00606 point_color.resize (3, 0); 00607 std::vector<unsigned int> nghbr_color; 00608 nghbr_color.resize (3, 0); 00609 point_color[0] = input_->points[point].r; 00610 point_color[1] = input_->points[point].g; 00611 point_color[2] = input_->points[point].b; 00612 nghbr_color[0] = input_->points[nghbr].r; 00613 nghbr_color[1] = input_->points[nghbr].g; 00614 nghbr_color[2] = input_->points[nghbr].b; 00615 float difference = calculateColorimetricalDifference (point_color, nghbr_color); 00616 if (difference > color_p2p_threshold_) 00617 return (false); 00618 00619 float cosine_threshold = cosf (theta_threshold_); 00620 00621 // check the angle between normals if needed 00622 if (normal_flag_) 00623 { 00624 float data[4]; 00625 data[0] = input_->points[point].data[0]; 00626 data[1] = input_->points[point].data[1]; 00627 data[2] = input_->points[point].data[2]; 00628 data[3] = input_->points[point].data[3]; 00629 00630 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data)); 00631 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal)); 00632 if (smooth_mode_flag_ == true) 00633 { 00634 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); 00635 float dot_product = fabsf (nghbr_normal.dot (initial_normal)); 00636 if (dot_product < cosine_threshold) 00637 return (false); 00638 } 00639 else 00640 { 00641 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); 00642 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal)); 00643 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal)); 00644 if (dot_product < cosine_threshold) 00645 return (false); 00646 } 00647 } 00648 00649 // check the curvature if needed 00650 if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) 00651 is_a_seed = false; 00652 00653 // check the residual if needed 00654 if (residual_flag_) 00655 { 00656 float data_p[4]; 00657 data_p[0] = input_->points[point].data[0]; 00658 data_p[1] = input_->points[point].data[1]; 00659 data_p[2] = input_->points[point].data[2]; 00660 data_p[3] = input_->points[point].data[3]; 00661 float data_n[4]; 00662 data_n[0] = input_->points[nghbr].data[0]; 00663 data_n[1] = input_->points[nghbr].data[1]; 00664 data_n[2] = input_->points[nghbr].data[2]; 00665 data_n[3] = input_->points[nghbr].data[3]; 00666 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data_n)); 00667 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data_p)); 00668 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal)); 00669 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point)); 00670 if (residual > residual_threshold_) 00671 is_a_seed = false; 00672 } 00673 00674 return (true); 00675 } 00676 00677 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00678 template <typename PointT, typename NormalT> void 00679 pcl::RegionGrowingRGB<PointT, NormalT>::getSegmentFromPoint (int index, pcl::PointIndices& cluster) 00680 { 00681 cluster.indices.clear (); 00682 00683 bool segmentation_is_possible = initCompute (); 00684 if ( !segmentation_is_possible ) 00685 { 00686 deinitCompute (); 00687 return; 00688 } 00689 00690 // first of all we need to find out if this point belongs to cloud 00691 bool point_was_found = false; 00692 int number_of_points = static_cast <int> (indices_->size ()); 00693 for (size_t point = 0; point < number_of_points; point++) 00694 if ( (*indices_)[point] == index) 00695 { 00696 point_was_found = true; 00697 break; 00698 } 00699 00700 if (point_was_found) 00701 { 00702 if (clusters_.empty ()) 00703 { 00704 clusters_.clear (); 00705 point_neighbours_.clear (); 00706 point_labels_.clear (); 00707 num_pts_in_segment_.clear (); 00708 point_distances_.clear (); 00709 segment_neighbours_.clear (); 00710 segment_distances_.clear (); 00711 segment_labels_.clear (); 00712 number_of_segments_ = 0; 00713 00714 segmentation_is_possible = prepareForSegmentation (); 00715 if ( !segmentation_is_possible ) 00716 { 00717 deinitCompute (); 00718 return; 00719 } 00720 00721 findPointNeighbours (); 00722 applySmoothRegionGrowingAlgorithm (); 00723 RegionGrowing<PointT, NormalT>::assembleRegions (); 00724 00725 findSegmentNeighbours (); 00726 applyRegionMergingAlgorithm (); 00727 } 00728 // if we have already made the segmentation, then find the segment 00729 // to which this point belongs 00730 std::vector <pcl::PointIndices>::iterator i_segment; 00731 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++) 00732 { 00733 bool segment_was_found = false; 00734 for (size_t i_point = 0; i_point < i_segment->indices.size (); i_point++) 00735 { 00736 if (i_segment->indices[i_point] == index) 00737 { 00738 segment_was_found = true; 00739 cluster.indices.clear (); 00740 cluster.indices.reserve (i_segment->indices.size ()); 00741 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices)); 00742 break; 00743 } 00744 } 00745 if (segment_was_found) 00746 { 00747 break; 00748 } 00749 }// next segment 00750 }// end if point was found 00751 00752 deinitCompute (); 00753 } 00754 00755 #endif // PCL_SEGMENTATION_REGION_GROWING_RGB_HPP_