organized_connected_component_segmentation.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  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  *
00037  *
00038  */
00039 
00040 #ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_CONNECTED_COMPONENT_SEGMENTATION_H_
00041 #define PCL_SEGMENTATION_IMPL_ORGANIZED_CONNECTED_COMPONENT_SEGMENTATION_H_
00042 
00043 #include <pcl/segmentation/organized_connected_component_segmentation.h>
00044 
00051 
00052 template<typename PointT, typename PointLT> void
00053 pcl::OrganizedConnectedComponentSegmentation<PointT, PointLT>::findLabeledRegionBoundary (int start_idx, PointCloudLPtr labels, pcl::PointIndices& boundary_indices)
00054 {
00055   boundary_indices.indices.clear ();
00056   int curr_idx = start_idx;
00057   int curr_x   = start_idx % labels->width;
00058   int curr_y   = start_idx / labels->width;
00059   unsigned label = labels->points[start_idx].label;
00060   
00061   // fill lookup table for next points to visit
00062   Neighbor directions [8] = {Neighbor(-1,  0,                 -1),
00063                              Neighbor(-1, -1, -labels->width - 1), 
00064                              Neighbor( 0, -1, -labels->width    ),
00065                              Neighbor( 1, -1, -labels->width + 1),
00066                              Neighbor( 1,  0,                  1),
00067                              Neighbor( 1,  1,  labels->width + 1),
00068                              Neighbor( 0,  1,  labels->width    ),
00069                              Neighbor(-1,  1,  labels->width - 1)};
00070   
00071   // find one pixel with other label in the neighborhood -> assume thats the one we came from
00072   int direction = -1;
00073   int x;
00074   int y;
00075   int index;
00076   for (unsigned dIdx = 0; dIdx < 8; ++dIdx)
00077   {
00078     x = curr_x + directions [dIdx].d_x;
00079     y = curr_y + directions [dIdx].d_y;
00080     index = curr_idx + directions [dIdx].d_index;
00081     if (x >= 0 && x < int(labels->width) && y >= 0 && y < int(labels->height) && labels->points[index].label != label)
00082     {
00083       direction = dIdx;
00084       break;
00085     }
00086   }
00087 
00088   // no connection to outer regions => start_idx is not on the border
00089   if (direction == -1)
00090     return;
00091   
00092   boundary_indices.indices.push_back (start_idx);
00093   
00094   do {
00095     unsigned nIdx;
00096     for (unsigned dIdx = 1; dIdx <= 8; ++dIdx)
00097     {
00098       nIdx = (direction + dIdx) & 7;
00099       
00100       x = curr_x + directions [nIdx].d_x;
00101       y = curr_y + directions [nIdx].d_y;
00102       index = curr_idx + directions [nIdx].d_index;
00103       if (x >= 0 && y < int(labels->width) && y >= 0 && y < int(labels->height) && labels->points[index].label == label)
00104         break;
00105     }
00106     
00107     // update the direction
00108     direction = (nIdx + 4) & 7;
00109     curr_idx += directions [nIdx].d_index;
00110     curr_x   += directions [nIdx].d_x;
00111     curr_y   += directions [nIdx].d_y;
00112     boundary_indices.indices.push_back(curr_idx);
00113   } while ( curr_idx != start_idx);
00114 }
00115 
00117 template<typename PointT, typename PointLT> void
00118 pcl::OrganizedConnectedComponentSegmentation<PointT, PointLT>::segment (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
00119 {
00120   std::vector<unsigned> run_ids;
00121 
00122   unsigned invalid_label = std::numeric_limits<unsigned>::max ();
00123   pcl::Label invalid_pt;
00124   invalid_pt.label = std::numeric_limits<unsigned>::max ();
00125   labels.points.resize (input_->points.size (), invalid_pt);
00126   labels.width = input_->width;
00127   labels.height = input_->height;
00128   unsigned int clust_id = 0;
00129   
00130   //First pixel
00131   if (pcl_isfinite (input_->points[0].x))
00132   {
00133     labels[0].label = clust_id++;
00134     run_ids.push_back (labels[0].label );
00135   }   
00136 
00137   // First row
00138   for (int colIdx = 1; colIdx < static_cast<int> (input_->width); ++colIdx)
00139   {
00140     if (!pcl_isfinite (input_->points[colIdx].x))
00141       continue;
00142     else if (compare_->compare (colIdx, colIdx - 1 ))
00143     {
00144       labels[colIdx].label = labels[colIdx - 1].label;
00145     }
00146     else
00147     {
00148       labels[colIdx].label = clust_id++;
00149       run_ids.push_back (labels[colIdx].label );
00150     }
00151   }
00152   
00153   // Everything else
00154   unsigned int current_row = input_->width;
00155   unsigned int previous_row = 0;
00156   for (size_t rowIdx = 1; rowIdx < input_->height; ++rowIdx, previous_row = current_row, current_row += input_->width)
00157   {
00158     // First pixel
00159     if (pcl_isfinite (input_->points[current_row].x))
00160     {
00161       if (compare_->compare (current_row, previous_row))
00162       {
00163         labels[current_row].label = labels[previous_row].label;
00164       }
00165       else
00166       {
00167         labels[current_row].label = clust_id++;
00168         run_ids.push_back (labels[current_row].label);
00169       }
00170     }
00171     
00172     // Rest of row
00173     for (int colIdx = 1; colIdx < static_cast<int> (input_->width); ++colIdx)
00174     {
00175       if (pcl_isfinite (input_->points[current_row + colIdx].x))
00176       {
00177         if (compare_->compare (current_row + colIdx, current_row + colIdx - 1))
00178         {
00179           labels[current_row + colIdx].label = labels[current_row + colIdx - 1].label;
00180         }
00181         if (compare_->compare (current_row + colIdx, previous_row + colIdx) )
00182         {
00183           if (labels[current_row + colIdx].label == invalid_label)
00184             labels[current_row + colIdx].label = labels[previous_row + colIdx].label;
00185           else
00186           {
00187             unsigned root1 = findRoot (run_ids, labels[current_row + colIdx].label);
00188             unsigned root2 = findRoot (run_ids, labels[previous_row + colIdx].label);
00189             
00190             if (root1 < root2)
00191               run_ids[root2] = root1;
00192             else
00193               run_ids[root1] = root2;
00194           }
00195         }
00196         
00197         if (labels[current_row + colIdx].label == invalid_label)
00198         {
00199           labels[current_row + colIdx].label = clust_id++;
00200           run_ids.push_back (labels[current_row + colIdx].label);
00201         }
00202         
00203       }
00204     }
00205   }
00206   
00207   std::vector<unsigned> map (clust_id);
00208   unsigned max_id = 0;
00209   for (unsigned runIdx = 0; runIdx < run_ids.size (); ++runIdx)
00210   {
00211     // if it is its own root -> new region
00212     if (run_ids[runIdx] == runIdx)
00213       map[runIdx] = max_id++;
00214     else // assign this sub-segment to the region (root) it belongs
00215       map [runIdx] = map [findRoot (run_ids, runIdx)];
00216   }
00217 
00218   label_indices.resize (max_id + 1);
00219   for (unsigned idx = 0; idx < input_->points.size (); idx++)
00220   {
00221     if (labels[idx].label != invalid_label)
00222     {
00223       labels[idx].label = map[labels[idx].label];
00224       label_indices[labels[idx].label].indices.push_back (idx);
00225     }
00226   }
00227 }
00228 
00229 #define PCL_INSTANTIATE_OrganizedConnectedComponentSegmentation(T,LT) template class PCL_EXPORTS pcl::OrganizedConnectedComponentSegmentation<T,LT>;
00230 
00231 #endif //#ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_CONNECTED_COMPONENT_SEGMENTATION_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:09