Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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 00045 /** 00046 * Directions: 1 2 3 00047 * 0 x 4 00048 * 7 6 5 00049 * e.g. direction y means we came from pixel with label y to the center pixel x 00050 */ 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 00116 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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_