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_HPP_ 00041 #define PCL_SEGMENTATION_REGION_GROWING_HPP_ 00042 00043 #include <pcl/segmentation/region_growing.h> 00044 00045 #include <pcl/search/search.h> 00046 #include <pcl/search/kdtree.h> 00047 #include <pcl/point_cloud.h> 00048 #include <pcl/point_types.h> 00049 00050 #include <queue> 00051 #include <list> 00052 #include <cmath> 00053 #include <time.h> 00054 00055 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00056 template <typename PointT, typename NormalT> 00057 pcl::RegionGrowing<PointT, NormalT>::RegionGrowing () : 00058 min_pts_per_cluster_ (1), 00059 max_pts_per_cluster_ (std::numeric_limits<int>::max ()), 00060 smooth_mode_flag_ (true), 00061 curvature_flag_ (true), 00062 residual_flag_ (false), 00063 theta_threshold_ (30.0f / 180.0f * static_cast<float> (M_PI)), 00064 residual_threshold_ (0.05f), 00065 curvature_threshold_ (0.05f), 00066 neighbour_number_ (30), 00067 search_ (), 00068 normals_ (), 00069 point_neighbours_ (0), 00070 point_labels_ (0), 00071 normal_flag_ (true), 00072 num_pts_in_segment_ (0), 00073 clusters_ (0), 00074 number_of_segments_ (0) 00075 { 00076 } 00077 00078 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00079 template <typename PointT, typename NormalT> 00080 pcl::RegionGrowing<PointT, NormalT>::~RegionGrowing () 00081 { 00082 if (search_ != 0) 00083 search_.reset (); 00084 if (normals_ != 0) 00085 normals_.reset (); 00086 00087 point_neighbours_.clear (); 00088 point_labels_.clear (); 00089 num_pts_in_segment_.clear (); 00090 clusters_.clear (); 00091 } 00092 00093 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00094 template <typename PointT, typename NormalT> int 00095 pcl::RegionGrowing<PointT, NormalT>::getMinClusterSize () 00096 { 00097 return (min_pts_per_cluster_); 00098 } 00099 00100 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00101 template <typename PointT, typename NormalT> void 00102 pcl::RegionGrowing<PointT, NormalT>::setMinClusterSize (int min_cluster_size) 00103 { 00104 min_pts_per_cluster_ = min_cluster_size; 00105 } 00106 00107 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00108 template <typename PointT, typename NormalT> int 00109 pcl::RegionGrowing<PointT, NormalT>::getMaxClusterSize () 00110 { 00111 return (max_pts_per_cluster_); 00112 } 00113 00114 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00115 template <typename PointT, typename NormalT> void 00116 pcl::RegionGrowing<PointT, NormalT>::setMaxClusterSize (int max_cluster_size) 00117 { 00118 max_pts_per_cluster_ = max_cluster_size; 00119 } 00120 00121 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00122 template <typename PointT, typename NormalT> bool 00123 pcl::RegionGrowing<PointT, NormalT>::getSmoothModeFlag () const 00124 { 00125 return (smooth_mode_flag_); 00126 } 00127 00128 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00129 template <typename PointT, typename NormalT> void 00130 pcl::RegionGrowing<PointT, NormalT>::setSmoothModeFlag (bool value) 00131 { 00132 smooth_mode_flag_ = value; 00133 } 00134 00135 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00136 template <typename PointT, typename NormalT> bool 00137 pcl::RegionGrowing<PointT, NormalT>::getCurvatureTestFlag () const 00138 { 00139 return (curvature_flag_); 00140 } 00141 00142 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00143 template <typename PointT, typename NormalT> void 00144 pcl::RegionGrowing<PointT, NormalT>::setCurvatureTestFlag (bool value) 00145 { 00146 curvature_flag_ = value; 00147 00148 if (curvature_flag_ == false && residual_flag_ == false) 00149 residual_flag_ = true; 00150 } 00151 00152 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00153 template <typename PointT, typename NormalT> bool 00154 pcl::RegionGrowing<PointT, NormalT>::getResidualTestFlag () const 00155 { 00156 return (residual_flag_); 00157 } 00158 00159 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00160 template <typename PointT, typename NormalT> void 00161 pcl::RegionGrowing<PointT, NormalT>::setResidualTestFlag (bool value) 00162 { 00163 residual_flag_ = value; 00164 00165 if (curvature_flag_ == false && residual_flag_ == false) 00166 curvature_flag_ = true; 00167 } 00168 00169 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00170 template <typename PointT, typename NormalT> float 00171 pcl::RegionGrowing<PointT, NormalT>::getSmoothnessThreshold () const 00172 { 00173 return (theta_threshold_); 00174 } 00175 00176 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00177 template <typename PointT, typename NormalT> void 00178 pcl::RegionGrowing<PointT, NormalT>::setSmoothnessThreshold (float theta) 00179 { 00180 theta_threshold_ = theta; 00181 } 00182 00183 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00184 template <typename PointT, typename NormalT> float 00185 pcl::RegionGrowing<PointT, NormalT>::getResidualThreshold () const 00186 { 00187 return (residual_threshold_); 00188 } 00189 00190 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00191 template <typename PointT, typename NormalT> void 00192 pcl::RegionGrowing<PointT, NormalT>::setResidualThreshold (float residual) 00193 { 00194 residual_threshold_ = residual; 00195 } 00196 00197 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00198 template <typename PointT, typename NormalT> float 00199 pcl::RegionGrowing<PointT, NormalT>::getCurvatureThreshold () const 00200 { 00201 return (curvature_threshold_); 00202 } 00203 00204 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00205 template <typename PointT, typename NormalT> void 00206 pcl::RegionGrowing<PointT, NormalT>::setCurvatureThreshold (float curvature) 00207 { 00208 curvature_threshold_ = curvature; 00209 } 00210 00211 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00212 template <typename PointT, typename NormalT> unsigned int 00213 pcl::RegionGrowing<PointT, NormalT>::getNumberOfNeighbours () const 00214 { 00215 return (neighbour_number_); 00216 } 00217 00218 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00219 template <typename PointT, typename NormalT> void 00220 pcl::RegionGrowing<PointT, NormalT>::setNumberOfNeighbours (unsigned int neighbour_number) 00221 { 00222 neighbour_number_ = neighbour_number; 00223 } 00224 00225 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00226 template <typename PointT, typename NormalT> typename pcl::RegionGrowing<PointT, NormalT>::KdTreePtr 00227 pcl::RegionGrowing<PointT, NormalT>::getSearchMethod () const 00228 { 00229 return (search_); 00230 } 00231 00232 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00233 template <typename PointT, typename NormalT> void 00234 pcl::RegionGrowing<PointT, NormalT>::setSearchMethod (const KdTreePtr& tree) 00235 { 00236 if (search_ != 0) 00237 search_.reset (); 00238 00239 search_ = tree; 00240 } 00241 00242 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00243 template <typename PointT, typename NormalT> typename pcl::RegionGrowing<PointT, NormalT>::NormalPtr 00244 pcl::RegionGrowing<PointT, NormalT>::getInputNormals () const 00245 { 00246 return (normals_); 00247 } 00248 00249 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00250 template <typename PointT, typename NormalT> void 00251 pcl::RegionGrowing<PointT, NormalT>::setInputNormals (const NormalPtr& norm) 00252 { 00253 if (normals_ != 0) 00254 normals_.reset (); 00255 00256 normals_ = norm; 00257 } 00258 00259 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00260 template <typename PointT, typename NormalT> void 00261 pcl::RegionGrowing<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters) 00262 { 00263 clusters_.clear (); 00264 clusters.clear (); 00265 point_neighbours_.clear (); 00266 point_labels_.clear (); 00267 num_pts_in_segment_.clear (); 00268 number_of_segments_ = 0; 00269 00270 bool segmentation_is_possible = initCompute (); 00271 if ( !segmentation_is_possible ) 00272 { 00273 deinitCompute (); 00274 return; 00275 } 00276 00277 segmentation_is_possible = prepareForSegmentation (); 00278 if ( !segmentation_is_possible ) 00279 { 00280 deinitCompute (); 00281 return; 00282 } 00283 00284 findPointNeighbours (); 00285 applySmoothRegionGrowingAlgorithm (); 00286 assembleRegions (); 00287 00288 clusters.resize (clusters_.size ()); 00289 std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin (); 00290 for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); cluster_iter++) 00291 { 00292 if ((cluster_iter->indices.size () >= min_pts_per_cluster_) && 00293 (cluster_iter->indices.size () <= max_pts_per_cluster_)) 00294 { 00295 *cluster_iter_input = *cluster_iter; 00296 cluster_iter_input++; 00297 } 00298 } 00299 00300 clusters_ = std::vector<pcl::PointIndices> (clusters.begin (), cluster_iter_input); 00301 00302 deinitCompute (); 00303 } 00304 00305 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00306 template <typename PointT, typename NormalT> bool 00307 pcl::RegionGrowing<PointT, NormalT>::prepareForSegmentation () 00308 { 00309 // if user forgot to pass point cloud or if it is empty 00310 if ( input_->points.size () == 0 ) 00311 return (false); 00312 00313 // if user forgot to pass normals or the sizes of point and normal cloud are different 00314 if ( normals_ == 0 || input_->points.size () != normals_->points.size () ) 00315 return (false); 00316 00317 // if residual test is on then we need to check if all needed parameters were correctly initialized 00318 if (residual_flag_) 00319 { 00320 if (residual_threshold_ <= 0.0f) 00321 return (false); 00322 } 00323 00324 // if curvature test is on ... 00325 // if (curvature_flag_) 00326 // { 00327 // in this case we do not need to check anything that related to it 00328 // so we simply commented it 00329 // } 00330 00331 // from here we check those parameters that are always valuable 00332 if (neighbour_number_ == 0) 00333 return (false); 00334 00335 // if user didn't set search method 00336 if (!search_) 00337 search_.reset (new pcl::search::KdTree<PointT>); 00338 00339 if (indices_) 00340 { 00341 if (indices_->empty ()) 00342 PCL_ERROR ("[pcl::RegionGrowing::prepareForSegmentation] Empty given indices!\n"); 00343 search_->setInputCloud (input_, indices_); 00344 } 00345 else 00346 search_->setInputCloud (input_); 00347 00348 return (true); 00349 } 00350 00351 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00352 template <typename PointT, typename NormalT> void 00353 pcl::RegionGrowing<PointT, NormalT>::findPointNeighbours () 00354 { 00355 int point_number = static_cast<int> (indices_->size ()); 00356 std::vector<int> neighbours; 00357 std::vector<float> distances; 00358 00359 point_neighbours_.resize (input_->points.size (), neighbours); 00360 00361 for (int i_point = 0; i_point < point_number; i_point++) 00362 { 00363 int point_index = (*indices_)[i_point]; 00364 neighbours.clear (); 00365 search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances); 00366 point_neighbours_[point_index].swap (neighbours); 00367 } 00368 } 00369 00370 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00371 template <typename PointT, typename NormalT> void 00372 pcl::RegionGrowing<PointT, NormalT>::applySmoothRegionGrowingAlgorithm () 00373 { 00374 int num_of_pts = static_cast<int> (indices_->size ()); 00375 point_labels_.resize (input_->points.size (), -1); 00376 00377 std::vector< std::pair<float, int> > point_residual; 00378 std::pair<float, int> pair; 00379 point_residual.resize (num_of_pts, pair); 00380 00381 if (normal_flag_ == true) 00382 { 00383 for (int i_point = 0; i_point < num_of_pts; i_point++) 00384 { 00385 int point_index = (*indices_)[i_point]; 00386 point_residual[i_point].first = normals_->points[point_index].curvature; 00387 point_residual[i_point].second = point_index; 00388 } 00389 std::sort (point_residual.begin (), point_residual.end (), comparePair); 00390 } 00391 else 00392 { 00393 for (int i_point = 0; i_point < num_of_pts; i_point++) 00394 { 00395 int point_index = (*indices_)[i_point]; 00396 point_residual[i_point].first = 0; 00397 point_residual[i_point].second = point_index; 00398 } 00399 } 00400 int seed_counter = 0; 00401 int seed = point_residual[seed_counter].second; 00402 00403 int segmented_pts_num = 0; 00404 int number_of_segments = 0; 00405 while (segmented_pts_num < num_of_pts) 00406 { 00407 int pts_in_segment; 00408 pts_in_segment = growRegion (seed, number_of_segments); 00409 segmented_pts_num += pts_in_segment; 00410 num_pts_in_segment_.push_back (pts_in_segment); 00411 number_of_segments++; 00412 00413 //find next point that is not segmented yet 00414 for (int i_seed = seed_counter + 1; i_seed < num_of_pts; i_seed++) 00415 { 00416 int index = point_residual[i_seed].second; 00417 if (point_labels_[index] == -1) 00418 { 00419 seed = index; 00420 break; 00421 } 00422 } 00423 } 00424 } 00425 00426 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00427 template <typename PointT, typename NormalT> int 00428 pcl::RegionGrowing<PointT, NormalT>::growRegion (int initial_seed, int segment_number) 00429 { 00430 std::queue<int> seeds; 00431 seeds.push (initial_seed); 00432 point_labels_[initial_seed] = segment_number; 00433 00434 int num_pts_in_segment = 1; 00435 00436 while (!seeds.empty ()) 00437 { 00438 int curr_seed; 00439 curr_seed = seeds.front (); 00440 seeds.pop (); 00441 00442 size_t i_nghbr = 0; 00443 while ( i_nghbr < neighbour_number_ && i_nghbr < point_neighbours_[curr_seed].size () ) 00444 { 00445 int index = point_neighbours_[curr_seed][i_nghbr]; 00446 if (point_labels_[index] != -1) 00447 { 00448 i_nghbr++; 00449 continue; 00450 } 00451 00452 bool is_a_seed = false; 00453 bool belongs_to_segment = validatePoint (initial_seed, curr_seed, index, is_a_seed); 00454 00455 if (belongs_to_segment == false) 00456 { 00457 i_nghbr++; 00458 continue; 00459 } 00460 00461 point_labels_[index] = segment_number; 00462 num_pts_in_segment++; 00463 00464 if (is_a_seed) 00465 { 00466 seeds.push (index); 00467 } 00468 00469 i_nghbr++; 00470 }// next neighbour 00471 }// next seed 00472 00473 return (num_pts_in_segment); 00474 } 00475 00476 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00477 template <typename PointT, typename NormalT> bool 00478 pcl::RegionGrowing<PointT, NormalT>::validatePoint (int initial_seed, int point, int nghbr, bool& is_a_seed) const 00479 { 00480 is_a_seed = true; 00481 00482 float cosine_threshold = cosf (theta_threshold_); 00483 float data[4]; 00484 00485 data[0] = input_->points[point].data[0]; 00486 data[1] = input_->points[point].data[1]; 00487 data[2] = input_->points[point].data[2]; 00488 data[3] = input_->points[point].data[3]; 00489 Eigen::Map<Eigen::Vector3f> initial_point (static_cast<float*> (data)); 00490 Eigen::Map<Eigen::Vector3f> initial_normal (static_cast<float*> (normals_->points[point].normal)); 00491 00492 //check the angle between normals 00493 if (smooth_mode_flag_ == true) 00494 { 00495 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); 00496 float dot_product = fabsf (nghbr_normal.dot (initial_normal)); 00497 if (dot_product < cosine_threshold) 00498 { 00499 return (false); 00500 } 00501 } 00502 else 00503 { 00504 Eigen::Map<Eigen::Vector3f> nghbr_normal (static_cast<float*> (normals_->points[nghbr].normal)); 00505 Eigen::Map<Eigen::Vector3f> initial_seed_normal (static_cast<float*> (normals_->points[initial_seed].normal)); 00506 float dot_product = fabsf (nghbr_normal.dot (initial_seed_normal)); 00507 if (dot_product < cosine_threshold) 00508 return (false); 00509 } 00510 00511 // check the curvature if needed 00512 if (curvature_flag_ && normals_->points[nghbr].curvature > curvature_threshold_) 00513 { 00514 is_a_seed = false; 00515 } 00516 00517 // check the residual if needed 00518 data[0] = input_->points[nghbr].data[0]; 00519 data[1] = input_->points[nghbr].data[1]; 00520 data[2] = input_->points[nghbr].data[2]; 00521 data[3] = input_->points[nghbr].data[3]; 00522 Eigen::Map<Eigen::Vector3f> nghbr_point (static_cast<float*> (data)); 00523 float residual = fabsf (initial_normal.dot (initial_point - nghbr_point)); 00524 if (residual_flag_ && residual > residual_threshold_) 00525 is_a_seed = false; 00526 00527 return (true); 00528 } 00529 00530 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00531 template <typename PointT, typename NormalT> void 00532 pcl::RegionGrowing<PointT, NormalT>::assembleRegions () 00533 { 00534 int number_of_segments = static_cast<int> (num_pts_in_segment_.size ()); 00535 int number_of_points = static_cast<int> (input_->points.size ()); 00536 00537 pcl::PointIndices segment; 00538 clusters_.resize (number_of_segments, segment); 00539 00540 for (int i_seg = 0; i_seg < number_of_segments; i_seg++) 00541 { 00542 clusters_[i_seg].indices.resize ( num_pts_in_segment_[i_seg], 0); 00543 } 00544 00545 std::vector<int> counter; 00546 counter.resize (number_of_segments, 0); 00547 00548 for (int i_point = 0; i_point < number_of_points; i_point++) 00549 { 00550 int segment_index = point_labels_[i_point]; 00551 if (segment_index != -1) 00552 { 00553 int point_index = counter[segment_index]; 00554 clusters_[segment_index].indices[point_index] = i_point; 00555 counter[segment_index] = point_index + 1; 00556 } 00557 } 00558 00559 number_of_segments_ = number_of_segments; 00560 } 00561 00562 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00563 template <typename PointT, typename NormalT> void 00564 pcl::RegionGrowing<PointT, NormalT>::getSegmentFromPoint (int index, pcl::PointIndices& cluster) 00565 { 00566 cluster.indices.clear (); 00567 00568 bool segmentation_is_possible = initCompute (); 00569 if ( !segmentation_is_possible ) 00570 { 00571 deinitCompute (); 00572 return; 00573 } 00574 00575 // first of all we need to find out if this point belongs to cloud 00576 bool point_was_found = false; 00577 int number_of_points = static_cast <int> (indices_->size ()); 00578 for (size_t point = 0; point < number_of_points; point++) 00579 if ( (*indices_)[point] == index) 00580 { 00581 point_was_found = true; 00582 break; 00583 } 00584 00585 if (point_was_found) 00586 { 00587 if (clusters_.empty ()) 00588 { 00589 point_neighbours_.clear (); 00590 point_labels_.clear (); 00591 num_pts_in_segment_.clear (); 00592 number_of_segments_ = 0; 00593 00594 segmentation_is_possible = prepareForSegmentation (); 00595 if ( !segmentation_is_possible ) 00596 { 00597 deinitCompute (); 00598 return; 00599 } 00600 00601 findPointNeighbours (); 00602 applySmoothRegionGrowingAlgorithm (); 00603 assembleRegions (); 00604 } 00605 // if we have already made the segmentation, then find the segment 00606 // to which this point belongs 00607 std::vector <pcl::PointIndices>::iterator i_segment; 00608 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++) 00609 { 00610 bool segment_was_found = false; 00611 for (size_t i_point = 0; i_point < i_segment->indices.size (); i_point++) 00612 { 00613 if (i_segment->indices[i_point] == index) 00614 { 00615 segment_was_found = true; 00616 cluster.indices.clear (); 00617 cluster.indices.reserve (i_segment->indices.size ()); 00618 std::copy (i_segment->indices.begin (), i_segment->indices.end (), std::back_inserter (cluster.indices)); 00619 break; 00620 } 00621 } 00622 if (segment_was_found) 00623 { 00624 break; 00625 } 00626 }// next segment 00627 }// end if point was found 00628 00629 deinitCompute (); 00630 } 00631 00632 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00633 template <typename PointT, typename NormalT> pcl::PointCloud<pcl::PointXYZRGB>::Ptr 00634 pcl::RegionGrowing<PointT, NormalT>::getColoredCloud () 00635 { 00636 pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud; 00637 00638 if (!clusters_.empty ()) 00639 { 00640 colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared (); 00641 00642 srand (static_cast<unsigned int> (time (0))); 00643 std::vector<unsigned char> colors; 00644 for (size_t i_segment = 0; i_segment < clusters_.size (); i_segment++) 00645 { 00646 colors.push_back (static_cast<unsigned char> (rand () % 256)); 00647 colors.push_back (static_cast<unsigned char> (rand () % 256)); 00648 colors.push_back (static_cast<unsigned char> (rand () % 256)); 00649 } 00650 00651 colored_cloud->width = input_->width; 00652 colored_cloud->height = input_->height; 00653 colored_cloud->is_dense = input_->is_dense; 00654 for (size_t i_point = 0; i_point < input_->points.size (); i_point++) 00655 { 00656 pcl::PointXYZRGB point; 00657 point.x = *(input_->points[i_point].data); 00658 point.y = *(input_->points[i_point].data + 1); 00659 point.z = *(input_->points[i_point].data + 2); 00660 point.r = 255; 00661 point.g = 0; 00662 point.b = 0; 00663 colored_cloud->points.push_back (point); 00664 } 00665 00666 std::vector< pcl::PointIndices >::iterator i_segment; 00667 int next_color = 0; 00668 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++) 00669 { 00670 std::vector<int>::iterator i_point; 00671 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++) 00672 { 00673 int index; 00674 index = *i_point; 00675 colored_cloud->points[index].r = colors[3 * next_color]; 00676 colored_cloud->points[index].g = colors[3 * next_color + 1]; 00677 colored_cloud->points[index].b = colors[3 * next_color + 2]; 00678 } 00679 next_color++; 00680 } 00681 } 00682 00683 return (colored_cloud); 00684 } 00685 00686 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00687 template <typename PointT, typename NormalT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr 00688 pcl::RegionGrowing<PointT, NormalT>::getColoredCloudRGBA () 00689 { 00690 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud; 00691 00692 if (!clusters_.empty ()) 00693 { 00694 colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGBA>)->makeShared (); 00695 00696 srand (static_cast<unsigned int> (time (0))); 00697 std::vector<unsigned char> colors; 00698 for (size_t i_segment = 0; i_segment < clusters_.size (); i_segment++) 00699 { 00700 colors.push_back (static_cast<unsigned char> (rand () % 256)); 00701 colors.push_back (static_cast<unsigned char> (rand () % 256)); 00702 colors.push_back (static_cast<unsigned char> (rand () % 256)); 00703 } 00704 00705 colored_cloud->width = input_->width; 00706 colored_cloud->height = input_->height; 00707 colored_cloud->is_dense = input_->is_dense; 00708 for (size_t i_point = 0; i_point < input_->points.size (); i_point++) 00709 { 00710 pcl::PointXYZRGBA point; 00711 point.x = *(input_->points[i_point].data); 00712 point.y = *(input_->points[i_point].data + 1); 00713 point.z = *(input_->points[i_point].data + 2); 00714 point.r = 255; 00715 point.g = 0; 00716 point.b = 0; 00717 point.a = 0; 00718 colored_cloud->points.push_back (point); 00719 } 00720 00721 std::vector< pcl::PointIndices >::iterator i_segment; 00722 int next_color = 0; 00723 for (i_segment = clusters_.begin (); i_segment != clusters_.end (); i_segment++) 00724 { 00725 std::vector<int>::iterator i_point; 00726 for (i_point = i_segment->indices.begin (); i_point != i_segment->indices.end (); i_point++) 00727 { 00728 int index; 00729 index = *i_point; 00730 colored_cloud->points[index].r = colors[3 * next_color]; 00731 colored_cloud->points[index].g = colors[3 * next_color + 1]; 00732 colored_cloud->points[index].b = colors[3 * next_color + 2]; 00733 } 00734 next_color++; 00735 } 00736 } 00737 00738 return (colored_cloud); 00739 } 00740 00741 #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing<T, pcl::Normal>; 00742 00743 #endif // PCL_SEGMENTATION_REGION_GROWING_HPP_