1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, 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 #ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
39 #define PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
40
41 #include <pcl/2d/edge.h>
42 #include <pcl/features/organized_edge_detection.h>
43
44 /**
45 * Directions: 1 2 3
46 * 0 x 4
47 * 7 6 5
48 * e.g. direction y means we came from pixel with label y to the center pixel x
49 */
50 //////////////////////////////////////////////////////////////////////////////
51 template<typename PointT, typename PointLT> void
compute(pcl::PointCloud<PointLT> & labels,std::vector<pcl::PointIndices> & label_indices) const52 pcl::OrganizedEdgeBase<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
53 {
54 pcl::Label invalid_pt;
55 invalid_pt.label = unsigned (0);
56 labels.resize (input_->size (), invalid_pt);
57 labels.width = input_->width;
58 labels.height = input_->height;
59
60 extractEdges (labels);
61
62 assignLabelIndices (labels, label_indices);
63 }
64
65 //////////////////////////////////////////////////////////////////////////////
66 template<typename PointT, typename PointLT> void
assignLabelIndices(pcl::PointCloud<PointLT> & labels,std::vector<pcl::PointIndices> & label_indices) const67 pcl::OrganizedEdgeBase<PointT, PointLT>::assignLabelIndices (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
68 {
69 const unsigned invalid_label = unsigned (0);
70 label_indices.resize (num_of_edgetype_);
71 for (std::size_t idx = 0; idx < input_->size (); idx++)
72 {
73 if (labels[idx].label != invalid_label)
74 {
75 for (int edge_type = 0; edge_type < num_of_edgetype_; edge_type++)
76 {
77 if ((labels[idx].label >> edge_type) & 1)
78 label_indices[edge_type].indices.push_back (idx);
79 }
80 }
81 }
82 }
83
84 //////////////////////////////////////////////////////////////////////////////
85 template<typename PointT, typename PointLT> void
extractEdges(pcl::PointCloud<PointLT> & labels) const86 pcl::OrganizedEdgeBase<PointT, PointLT>::extractEdges (pcl::PointCloud<PointLT>& labels) const
87 {
88 if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED))
89 {
90 // Fill lookup table for next points to visit
91 const int num_of_ngbr = 8;
92 Neighbor directions [num_of_ngbr] = {Neighbor(-1, 0, -1),
93 Neighbor(-1, -1, -labels.width - 1),
94 Neighbor( 0, -1, -labels.width ),
95 Neighbor( 1, -1, -labels.width + 1),
96 Neighbor( 1, 0, 1),
97 Neighbor( 1, 1, labels.width + 1),
98 Neighbor( 0, 1, labels.width ),
99 Neighbor(-1, 1, labels.width - 1)};
100
101 for (int row = 1; row < int(input_->height) - 1; row++)
102 {
103 for (int col = 1; col < int(input_->width) - 1; col++)
104 {
105 int curr_idx = row*int(input_->width) + col;
106 if (!std::isfinite ((*input_)[curr_idx].z))
107 continue;
108
109 float curr_depth = std::abs ((*input_)[curr_idx].z);
110
111 // Calculate depth distances between current point and neighboring points
112 std::vector<float> nghr_dist;
113 nghr_dist.resize (8);
114 bool found_invalid_neighbor = false;
115 for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
116 {
117 int nghr_idx = curr_idx + directions[d_idx].d_index;
118 assert (nghr_idx >= 0 && static_cast<std::size_t>(nghr_idx) < input_->size ());
119 if (!std::isfinite ((*input_)[nghr_idx].z))
120 {
121 found_invalid_neighbor = true;
122 break;
123 }
124 nghr_dist[d_idx] = curr_depth - std::abs ((*input_)[nghr_idx].z);
125 }
126
127 if (!found_invalid_neighbor)
128 {
129 // Every neighboring points are valid
130 std::vector<float>::const_iterator min_itr, max_itr;
131 std::tie (min_itr, max_itr) = std::minmax_element (nghr_dist.cbegin (), nghr_dist.cend ());
132 float nghr_dist_min = *min_itr;
133 float nghr_dist_max = *max_itr;
134 float dist_dominant = std::abs (nghr_dist_min) > std::abs (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
135 if (std::abs (dist_dominant) > th_depth_discon_*std::abs (curr_depth))
136 {
137 // Found a depth discontinuity
138 if (dist_dominant > 0.f)
139 {
140 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
141 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
142 }
143 else
144 {
145 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
146 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
147 }
148 }
149 }
150 else
151 {
152 // Some neighboring points are not valid (nan points)
153 // Search for corresponding point across invalid points
154 // Search direction is determined by nan point locations with respect to current point
155 int dx = 0;
156 int dy = 0;
157 int num_of_invalid_pt = 0;
158 for (const auto &direction : directions)
159 {
160 int nghr_idx = curr_idx + direction.d_index;
161 assert (nghr_idx >= 0 && static_cast<std::size_t>(nghr_idx) < input_->size ());
162 if (!std::isfinite ((*input_)[nghr_idx].z))
163 {
164 dx += direction.d_x;
165 dy += direction.d_y;
166 num_of_invalid_pt++;
167 }
168 }
169
170 // Search directions
171 assert (num_of_invalid_pt > 0);
172 float f_dx = static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
173 float f_dy = static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);
174
175 // Search for corresponding point across invalid points
176 float corr_depth = std::numeric_limits<float>::quiet_NaN ();
177 for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
178 {
179 int s_row = row + static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
180 int s_col = col + static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));
181
182 if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width))
183 break;
184
185 if (std::isfinite ((*input_)[s_row*int(input_->width)+s_col].z))
186 {
187 corr_depth = std::abs ((*input_)[s_row*int(input_->width)+s_col].z);
188 break;
189 }
190 }
191
192 if (!std::isnan (corr_depth))
193 {
194 // Found a corresponding point
195 float dist = curr_depth - corr_depth;
196 if (std::abs (dist) > th_depth_discon_*std::abs (curr_depth))
197 {
198 // Found a depth discontinuity
199 if (dist > 0.f)
200 {
201 if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
202 labels[curr_idx].label |= EDGELABEL_OCCLUDED;
203 }
204 else
205 {
206 if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
207 labels[curr_idx].label |= EDGELABEL_OCCLUDING;
208 }
209 }
210 }
211 else
212 {
213 // Not found a corresponding point, just nan boundary edge
214 if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY)
215 labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY;
216 }
217 }
218 }
219 }
220 }
221 }
222
223
224 //////////////////////////////////////////////////////////////////////////////
225 template<typename PointT, typename PointLT> void
compute(pcl::PointCloud<PointLT> & labels,std::vector<pcl::PointIndices> & label_indices) const226 pcl::OrganizedEdgeFromRGB<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
227 {
228 pcl::Label invalid_pt;
229 invalid_pt.label = unsigned (0);
230 labels.resize (input_->size (), invalid_pt);
231 labels.width = input_->width;
232 labels.height = input_->height;
233
234 OrganizedEdgeBase<PointT, PointLT>::extractEdges (labels);
235 extractEdges (labels);
236
237 this->assignLabelIndices (labels, label_indices);
238 }
239
240 //////////////////////////////////////////////////////////////////////////////
241 template<typename PointT, typename PointLT> void
extractEdges(pcl::PointCloud<PointLT> & labels) const242 pcl::OrganizedEdgeFromRGB<PointT, PointLT>::extractEdges (pcl::PointCloud<PointLT>& labels) const
243 {
244 if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY))
245 {
246 pcl::PointCloud<PointXYZI>::Ptr gray (new pcl::PointCloud<PointXYZI>);
247 gray->width = input_->width;
248 gray->height = input_->height;
249 gray->resize (input_->height*input_->width);
250
251 for (std::size_t i = 0; i < input_->size (); ++i)
252 (*gray)[i].intensity = float (((*input_)[i].r + (*input_)[i].g + (*input_)[i].b) / 3);
253
254 pcl::PointCloud<pcl::PointXYZIEdge> img_edge_rgb;
255 pcl::Edge<PointXYZI, pcl::PointXYZIEdge> edge;
256 edge.setInputCloud (gray);
257 edge.setHysteresisThresholdLow (th_rgb_canny_low_);
258 edge.setHysteresisThresholdHigh (th_rgb_canny_high_);
259 edge.detectEdgeCanny (img_edge_rgb);
260
261 for (std::uint32_t row=0; row<labels.height; row++)
262 {
263 for (std::uint32_t col=0; col<labels.width; col++)
264 {
265 if (img_edge_rgb (col, row).magnitude == 255.f)
266 labels[row * labels.width + col].label |= EDGELABEL_RGB_CANNY;
267 }
268 }
269 }
270 }
271
272 //////////////////////////////////////////////////////////////////////////////
273 template<typename PointT, typename PointNT, typename PointLT> void
compute(pcl::PointCloud<PointLT> & labels,std::vector<pcl::PointIndices> & label_indices) const274 pcl::OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
275 {
276 pcl::Label invalid_pt;
277 invalid_pt.label = unsigned (0);
278 labels.resize (input_->size (), invalid_pt);
279 labels.width = input_->width;
280 labels.height = input_->height;
281
282 OrganizedEdgeBase<PointT, PointLT>::extractEdges (labels);
283 extractEdges (labels);
284
285 this->assignLabelIndices (labels, label_indices);
286 }
287
288 //////////////////////////////////////////////////////////////////////////////
289 template<typename PointT, typename PointNT, typename PointLT> void
extractEdges(pcl::PointCloud<PointLT> & labels) const290 pcl::OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::extractEdges (pcl::PointCloud<PointLT>& labels) const
291 {
292 if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE))
293 {
294
295 pcl::PointCloud<PointXYZI> nx, ny;
296 nx.width = normals_->width;
297 nx.height = normals_->height;
298 nx.resize (normals_->height*normals_->width);
299
300 ny.width = normals_->width;
301 ny.height = normals_->height;
302 ny.resize (normals_->height*normals_->width);
303
304 for (std::uint32_t row=0; row<normals_->height; row++)
305 {
306 for (std::uint32_t col=0; col<normals_->width; col++)
307 {
308 nx (col, row).intensity = (*normals_)[row*normals_->width + col].normal_x;
309 ny (col, row).intensity = (*normals_)[row*normals_->width + col].normal_y;
310 }
311 }
312
313 pcl::PointCloud<pcl::PointXYZIEdge> img_edge;
314 pcl::Edge<PointXYZI, pcl::PointXYZIEdge> edge;
315 edge.setHysteresisThresholdLow (th_hc_canny_low_);
316 edge.setHysteresisThresholdHigh (th_hc_canny_high_);
317 edge.canny (nx, ny, img_edge);
318
319 for (std::uint32_t row=0; row<labels.height; row++)
320 {
321 for (std::uint32_t col=0; col<labels.width; col++)
322 {
323 if (img_edge (col, row).magnitude == 255.f)
324 labels[row * labels.width + col].label |= EDGELABEL_HIGH_CURVATURE;
325 }
326 }
327 }
328 }
329
330 //////////////////////////////////////////////////////////////////////////////
331 template<typename PointT, typename PointNT, typename PointLT> void
compute(pcl::PointCloud<PointLT> & labels,std::vector<pcl::PointIndices> & label_indices) const332 pcl::OrganizedEdgeFromRGBNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
333 {
334 pcl::Label invalid_pt;
335 invalid_pt.label = unsigned (0);
336 labels.resize (input_->size (), invalid_pt);
337 labels.width = input_->width;
338 labels.height = input_->height;
339
340 OrganizedEdgeBase<PointT, PointLT>::extractEdges (labels);
341 OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::extractEdges (labels);
342 OrganizedEdgeFromRGB<PointT, PointLT>::extractEdges (labels);
343
344 this->assignLabelIndices (labels, label_indices);
345 }
346
347 #define PCL_INSTANTIATE_OrganizedEdgeBase(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeBase<T,LT>;
348 #define PCL_INSTANTIATE_OrganizedEdgeFromRGB(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGB<T,LT>;
349 #define PCL_INSTANTIATE_OrganizedEdgeFromNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromNormals<T,NT,LT>;
350 #define PCL_INSTANTIATE_OrganizedEdgeFromRGBNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGBNormals<T,NT,LT>;
351
352 #endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
353