Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
organized_connected_component_segmentation.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 *
37 *
38 */
39
40#ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_CONNECTED_COMPONENT_SEGMENTATION_H_
41#define PCL_SEGMENTATION_IMPL_ORGANIZED_CONNECTED_COMPONENT_SEGMENTATION_H_
42
43#include <pcl/segmentation/organized_connected_component_segmentation.h>
44
45/**
46 * Directions: 1 2 3
47 * 0 x 4
48 * 7 6 5
49 * e.g. direction y means we came from pixel with label y to the center pixel x
50 */
51////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
52template<typename PointT, typename PointLT> void
54{
55 boundary_indices.indices.clear ();
56 int curr_idx = start_idx;
57 int curr_x = start_idx % labels->width;
58 int curr_y = start_idx / labels->width;
59 unsigned label = (*labels)[start_idx].label;
60
61 // fill lookup table for next points to visit
62 Neighbor directions [8] = {Neighbor(-1, 0, -1),
63 Neighbor(-1, -1, -labels->width - 1),
64 Neighbor( 0, -1, -labels->width ),
65 Neighbor( 1, -1, -labels->width + 1),
66 Neighbor( 1, 0, 1),
67 Neighbor( 1, 1, labels->width + 1),
68 Neighbor( 0, 1, labels->width ),
69 Neighbor(-1, 1, labels->width - 1)};
70
71 // find one pixel with other label in the neighborhood -> assume that's the one we came from
72 int direction = -1;
73 int x;
74 int y;
75 int index;
76 for (unsigned dIdx = 0; dIdx < 8; ++dIdx)
77 {
78 x = curr_x + directions [dIdx].d_x;
79 y = curr_y + directions [dIdx].d_y;
80 index = curr_idx + directions [dIdx].d_index;
81 if (x >= 0 && x < int(labels->width) && y >= 0 && y < int(labels->height) && (*labels)[index].label != label)
82 {
83 direction = dIdx;
84 break;
85 }
86 }
87
88 // no connection to outer regions => start_idx is not on the border
89 if (direction == -1)
90 return;
91
92 boundary_indices.indices.push_back (start_idx);
93
94 do {
95 unsigned nIdx;
96 for (unsigned dIdx = 1; dIdx <= 8; ++dIdx)
97 {
98 nIdx = (direction + dIdx) & 7;
99
100 x = curr_x + directions [nIdx].d_x;
101 y = curr_y + directions [nIdx].d_y;
102 index = curr_idx + directions [nIdx].d_index;
103 if (x >= 0 && x < int(labels->width) && y >= 0 && y < int(labels->height) && (*labels)[index].label == label)
104 break;
105 }
106
107 // update the direction
108 direction = (nIdx + 4) & 7;
109 curr_idx += directions [nIdx].d_index;
110 curr_x += directions [nIdx].d_x;
111 curr_y += directions [nIdx].d_y;
112 boundary_indices.indices.push_back(curr_idx);
113 } while ( curr_idx != start_idx);
114}
115
116//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
117template<typename PointT, typename PointLT> void
119{
120 std::vector<unsigned> run_ids;
121
122 unsigned invalid_label = std::numeric_limits<unsigned>::max ();
123 PointLT invalid_pt;
124 invalid_pt.label = std::numeric_limits<unsigned>::max ();
125 labels.resize (input_->size (), invalid_pt);
126 labels.width = input_->width;
127 labels.height = input_->height;
128 std::size_t clust_id = 0;
129
130 //First pixel
131 if (std::isfinite ((*input_)[0].x))
132 {
133 labels[0].label = clust_id++;
134 run_ids.push_back (labels[0].label );
135 }
136
137 // First row
138 for (int colIdx = 1; colIdx < static_cast<int> (input_->width); ++colIdx)
139 {
140 if (!std::isfinite ((*input_)[colIdx].x))
141 continue;
142 if (compare_->compare (colIdx, colIdx - 1 ))
143 {
144 labels[colIdx].label = labels[colIdx - 1].label;
145 }
146 else
147 {
148 labels[colIdx].label = clust_id++;
149 run_ids.push_back (labels[colIdx].label );
150 }
151 }
152
153 // Everything else
154 unsigned int current_row = input_->width;
155 unsigned int previous_row = 0;
156 for (std::size_t rowIdx = 1; rowIdx < input_->height; ++rowIdx, previous_row = current_row, current_row += input_->width)
157 {
158 // First pixel
159 if (std::isfinite ((*input_)[current_row].x))
160 {
161 if (compare_->compare (current_row, previous_row))
162 {
163 labels[current_row].label = labels[previous_row].label;
164 }
165 else
166 {
167 labels[current_row].label = clust_id++;
168 run_ids.push_back (labels[current_row].label);
169 }
170 }
171
172 // Rest of row
173 for (int colIdx = 1; colIdx < static_cast<int> (input_->width); ++colIdx)
174 {
175 if (std::isfinite ((*input_)[current_row + colIdx].x))
176 {
177 if (compare_->compare (current_row + colIdx, current_row + colIdx - 1))
178 {
179 labels[current_row + colIdx].label = labels[current_row + colIdx - 1].label;
180 }
181 if (compare_->compare (current_row + colIdx, previous_row + colIdx) )
182 {
183 if (labels[current_row + colIdx].label == invalid_label)
184 labels[current_row + colIdx].label = labels[previous_row + colIdx].label;
185 else if (labels[previous_row + colIdx].label != invalid_label)
186 {
187 unsigned root1 = findRoot (run_ids, labels[current_row + colIdx].label);
188 unsigned root2 = findRoot (run_ids, labels[previous_row + colIdx].label);
189
190 if (root1 < root2)
191 run_ids[root2] = root1;
192 else
193 run_ids[root1] = root2;
194 }
195 }
196
197 if (labels[current_row + colIdx].label == invalid_label)
198 {
199 labels[current_row + colIdx].label = clust_id++;
200 run_ids.push_back (labels[current_row + colIdx].label);
201 }
202
203 }
204 }
205 }
206
207 std::vector<unsigned> map (clust_id);
208 std::size_t max_id = 0;
209 for (std::size_t runIdx = 0; runIdx < run_ids.size (); ++runIdx)
210 {
211 // if it is its own root -> new region
212 if (run_ids[runIdx] == runIdx)
213 map[runIdx] = max_id++;
214 else // assign this sub-segment to the region (root) it belongs
215 map [runIdx] = map [findRoot (run_ids, runIdx)];
216 }
217
218 label_indices.resize (max_id + 1);
219 for (std::size_t idx = 0; idx < input_->size (); idx++)
220 {
221 if (labels[idx].label != invalid_label)
222 {
223 labels[idx].label = map[labels[idx].label];
224 label_indices[labels[idx].label].indices.push_back (idx);
225 }
226 }
227}
228
229#define PCL_INSTANTIATE_OrganizedConnectedComponentSegmentation(T,LT) template class PCL_EXPORTS pcl::OrganizedConnectedComponentSegmentation<T,LT>;
230
231#endif //#ifndef PCL_SEGMENTATION_IMPL_ORGANIZED_CONNECTED_COMPONENT_SEGMENTATION_H_
static void findLabeledRegionBoundary(int start_idx, PointCloudLPtr labels, pcl::PointIndices &boundary_indices)
Find the boundary points / contour of a connected component.
void segment(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the connected component segmentation.
PointCloud represents the base class in PCL for storing collections of 3D points.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).