region_growing_rgb.hpp
Go to the documentation of this file.
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 
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 
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 
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 
00085 template <typename PointT, typename NormalT> void
00086 pcl::RegionGrowingRGB<PointT, NormalT>::setPointColorThreshold (float thresh)
00087 {
00088   color_p2p_threshold_ = thresh * thresh;
00089 }
00090 
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 
00099 template <typename PointT, typename NormalT> void
00100 pcl::RegionGrowingRGB<PointT, NormalT>::setRegionColorThreshold (float thresh)
00101 {
00102   color_r2r_threshold_ = thresh * thresh;
00103 }
00104 
00106 template <typename PointT, typename NormalT> float
00107 pcl::RegionGrowingRGB<PointT, NormalT>::getDistanceThreshold () const
00108 {
00109   return (powf (distance_threshold_, 0.5f));
00110 }
00111 
00113 template <typename PointT, typename NormalT> void
00114 pcl::RegionGrowingRGB<PointT, NormalT>::setDistanceThreshold (float thresh)
00115 {
00116   distance_threshold_ = thresh * thresh;
00117 }
00118 
00120 template <typename PointT, typename NormalT> unsigned int
00121 pcl::RegionGrowingRGB<PointT, NormalT>::getNumberOfRegionNeighbours () const
00122 {
00123   return (region_neighbour_number_);
00124 }
00125 
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 
00134 template <typename PointT, typename NormalT> bool
00135 pcl::RegionGrowingRGB<PointT, NormalT>::getNormalTestFlag () const
00136 {
00137   return (normal_flag_);
00138 }
00139 
00141 template <typename PointT, typename NormalT> void
00142 pcl::RegionGrowingRGB<PointT, NormalT>::setNormalTestFlag (bool value)
00143 {
00144   normal_flag_ = value;
00145 }
00146 
00148 template <typename PointT, typename NormalT> void
00149 pcl::RegionGrowingRGB<PointT, NormalT>::setCurvatureTestFlag (bool value)
00150 {
00151   curvature_flag_ = value;
00152 }
00153 
00155 template <typename PointT, typename NormalT> void
00156 pcl::RegionGrowingRGB<PointT, NormalT>::setResidualTestFlag (bool value)
00157 {
00158   residual_flag_ = value;
00159 }
00160 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:02