Point Cloud Library (PCL)  1.8.0
organized_edge_detection.hpp
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 #include <pcl/console/print.h>
44 #include <pcl/console/time.h>
45 
46 /**
47  * Directions: 1 2 3
48  * 0 x 4
49  * 7 6 5
50  * e.g. direction y means we came from pixel with label y to the center pixel x
51  */
52 //////////////////////////////////////////////////////////////////////////////
53 template<typename PointT, typename PointLT> void
54 pcl::OrganizedEdgeBase<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
55 {
56  pcl::Label invalid_pt;
57  invalid_pt.label = unsigned (0);
58  labels.points.resize (input_->points.size (), invalid_pt);
59  labels.width = input_->width;
60  labels.height = input_->height;
61 
62  extractEdges (labels);
63 
64  assignLabelIndices (labels, label_indices);
65 }
66 
67 //////////////////////////////////////////////////////////////////////////////
68 template<typename PointT, typename PointLT> void
69 pcl::OrganizedEdgeBase<PointT, PointLT>::assignLabelIndices (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
70 {
71  const unsigned invalid_label = unsigned (0);
72  label_indices.resize (num_of_edgetype_);
73  for (unsigned idx = 0; idx < input_->points.size (); idx++)
74  {
75  if (labels[idx].label != invalid_label)
76  {
77  for (int edge_type = 0; edge_type < num_of_edgetype_; edge_type++)
78  {
79  if ((labels[idx].label >> edge_type) & 1)
80  label_indices[edge_type].indices.push_back (idx);
81  }
82  }
83  }
84 }
85 
86 //////////////////////////////////////////////////////////////////////////////
87 template<typename PointT, typename PointLT> void
89 {
90  if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED))
91  {
92  // Fill lookup table for next points to visit
93  const int num_of_ngbr = 8;
94  Neighbor directions [num_of_ngbr] = {Neighbor(-1, 0, -1),
95  Neighbor(-1, -1, -labels.width - 1),
96  Neighbor( 0, -1, -labels.width ),
97  Neighbor( 1, -1, -labels.width + 1),
98  Neighbor( 1, 0, 1),
99  Neighbor( 1, 1, labels.width + 1),
100  Neighbor( 0, 1, labels.width ),
101  Neighbor(-1, 1, labels.width - 1)};
102 
103  for (int row = 1; row < int(input_->height) - 1; row++)
104  {
105  for (int col = 1; col < int(input_->width) - 1; col++)
106  {
107  int curr_idx = row*int(input_->width) + col;
108  if (!pcl_isfinite (input_->points[curr_idx].z))
109  continue;
110 
111  float curr_depth = fabsf (input_->points[curr_idx].z);
112 
113  // Calculate depth distances between current point and neighboring points
114  std::vector<float> nghr_dist;
115  nghr_dist.resize (8);
116  bool found_invalid_neighbor = false;
117  for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
118  {
119  int nghr_idx = curr_idx + directions[d_idx].d_index;
120  assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
121  if (!pcl_isfinite (input_->points[nghr_idx].z))
122  {
123  found_invalid_neighbor = true;
124  break;
125  }
126  nghr_dist[d_idx] = curr_depth - fabsf (input_->points[nghr_idx].z);
127  }
128 
129  if (!found_invalid_neighbor)
130  {
131  // Every neighboring points are valid
132  std::vector<float>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());
133  std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());
134  float nghr_dist_min = *min_itr;
135  float nghr_dist_max = *max_itr;
136  float dist_dominant = fabsf (nghr_dist_min) > fabsf (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
137  if (fabsf (dist_dominant) > th_depth_discon_*fabsf (curr_depth))
138  {
139  // Found a depth discontinuity
140  if (dist_dominant > 0.f)
141  {
142  if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
143  labels[curr_idx].label |= EDGELABEL_OCCLUDED;
144  }
145  else
146  {
147  if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
148  labels[curr_idx].label |= EDGELABEL_OCCLUDING;
149  }
150  }
151  }
152  else
153  {
154  // Some neighboring points are not valid (nan points)
155  // Search for corresponding point across invalid points
156  // Search direction is determined by nan point locations with respect to current point
157  int dx = 0;
158  int dy = 0;
159  int num_of_invalid_pt = 0;
160  for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
161  {
162  int nghr_idx = curr_idx + directions[d_idx].d_index;
163  assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
164  if (!pcl_isfinite (input_->points[nghr_idx].z))
165  {
166  dx += directions[d_idx].d_x;
167  dy += directions[d_idx].d_y;
168  num_of_invalid_pt++;
169  }
170  }
171 
172  // Search directions
173  assert (num_of_invalid_pt > 0);
174  float f_dx = static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
175  float f_dy = static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);
176 
177  // Search for corresponding point across invalid points
178  float corr_depth = std::numeric_limits<float>::quiet_NaN ();
179  for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
180  {
181  int s_row = row + static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
182  int s_col = col + static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));
183 
184  if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width))
185  break;
186 
187  if (pcl_isfinite (input_->points[s_row*int(input_->width)+s_col].z))
188  {
189  corr_depth = fabsf (input_->points[s_row*int(input_->width)+s_col].z);
190  break;
191  }
192  }
193 
194  if (!pcl_isnan (corr_depth))
195  {
196  // Found a corresponding point
197  float dist = curr_depth - corr_depth;
198  if (fabsf (dist) > th_depth_discon_*fabsf (curr_depth))
199  {
200  // Found a depth discontinuity
201  if (dist > 0.f)
202  {
203  if (detecting_edge_types_ & EDGELABEL_OCCLUDED)
204  labels[curr_idx].label |= EDGELABEL_OCCLUDED;
205  }
206  else
207  {
208  if (detecting_edge_types_ & EDGELABEL_OCCLUDING)
209  labels[curr_idx].label |= EDGELABEL_OCCLUDING;
210  }
211  }
212  }
213  else
214  {
215  // Not found a corresponding point, just nan boundary edge
216  if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY)
217  labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY;
218  }
219  }
220  }
221  }
222  }
223 }
224 
225 
226 //////////////////////////////////////////////////////////////////////////////
227 template<typename PointT, typename PointLT> void
228 pcl::OrganizedEdgeFromRGB<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
229 {
230  pcl::Label invalid_pt;
231  invalid_pt.label = unsigned (0);
232  labels.points.resize (input_->points.size (), invalid_pt);
233  labels.width = input_->width;
234  labels.height = input_->height;
235 
237  extractEdges (labels);
238 
239  this->assignLabelIndices (labels, label_indices);
240 }
241 
242 //////////////////////////////////////////////////////////////////////////////
243 template<typename PointT, typename PointLT> void
245 {
246  if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY))
247  {
249  gray->width = input_->width;
250  gray->height = input_->height;
251  gray->resize (input_->height*input_->width);
252 
253  for (size_t i = 0; i < input_->size (); ++i)
254  (*gray)[i].intensity = float (((*input_)[i].r + (*input_)[i].g + (*input_)[i].b) / 3);
255 
258  edge.setInputCloud (gray);
259  edge.setHysteresisThresholdLow (th_rgb_canny_low_);
260  edge.setHysteresisThresholdHigh (th_rgb_canny_high_);
261  edge.detectEdgeCanny (img_edge_rgb);
262 
263  for (uint32_t row=0; row<labels.height; row++)
264  {
265  for (uint32_t col=0; col<labels.width; col++)
266  {
267  if (img_edge_rgb (col, row).magnitude == 255.f)
268  labels[row * labels.width + col].label |= EDGELABEL_RGB_CANNY;
269  }
270  }
271  }
272 }
273 
274 //////////////////////////////////////////////////////////////////////////////
275 template<typename PointT, typename PointNT, typename PointLT> void
276 pcl::OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
277 {
278  pcl::Label invalid_pt;
279  invalid_pt.label = unsigned (0);
280  labels.points.resize (input_->points.size (), invalid_pt);
281  labels.width = input_->width;
282  labels.height = input_->height;
283 
285  extractEdges (labels);
286 
287  this->assignLabelIndices (labels, label_indices);
288 }
289 
290 //////////////////////////////////////////////////////////////////////////////
291 template<typename PointT, typename PointNT, typename PointLT> void
293 {
294  if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE))
295  {
296 
298  nx.width = normals_->width;
299  nx.height = normals_->height;
300  nx.resize (normals_->height*normals_->width);
301 
302  ny.width = normals_->width;
303  ny.height = normals_->height;
304  ny.resize (normals_->height*normals_->width);
305 
306  for (uint32_t row=0; row<normals_->height; row++)
307  {
308  for (uint32_t col=0; col<normals_->width; col++)
309  {
310  nx (col, row).intensity = normals_->points[row*normals_->width + col].normal_x;
311  ny (col, row).intensity = normals_->points[row*normals_->width + col].normal_y;
312  }
313  }
314 
317  edge.setHysteresisThresholdLow (th_hc_canny_low_);
318  edge.setHysteresisThresholdHigh (th_hc_canny_high_);
319  edge.canny (nx, ny, img_edge);
320 
321  for (uint32_t row=0; row<labels.height; row++)
322  {
323  for (uint32_t col=0; col<labels.width; col++)
324  {
325  if (img_edge (col, row).magnitude == 255.f)
326  labels[row * labels.width + col].label |= EDGELABEL_HIGH_CURVATURE;
327  }
328  }
329  }
330 }
331 
332 //////////////////////////////////////////////////////////////////////////////
333 template<typename PointT, typename PointNT, typename PointLT> void
334 pcl::OrganizedEdgeFromRGBNormals<PointT, PointNT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
335 {
336  pcl::Label invalid_pt;
337  invalid_pt.label = unsigned (0);
338  labels.points.resize (input_->points.size (), invalid_pt);
339  labels.width = input_->width;
340  labels.height = input_->height;
341 
345 
346  this->assignLabelIndices (labels, label_indices);
347 }
348 
349 #define PCL_INSTANTIATE_OrganizedEdgeBase(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeBase<T,LT>;
350 #define PCL_INSTANTIATE_OrganizedEdgeFromRGB(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGB<T,LT>;
351 #define PCL_INSTANTIATE_OrganizedEdgeFromNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromNormals<T,NT,LT>;
352 #define PCL_INSTANTIATE_OrganizedEdgeFromRGBNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGBNormals<T,NT,LT>;
353 
354 #endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void setHysteresisThresholdHigh(float threshold)
Definition: edge.h:153
Definition: edge.h:48
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void setInputCloud(PointCloudInPtr input)
Set the input point cloud pointer.
Definition: edge.h:297
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities, RGB Canny edge, and high curvature r...
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) and assign point ...
uint32_t label
void setHysteresisThresholdLow(float threshold)
Definition: edge.h:147
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) ...
void canny(const pcl::PointCloud< PointInT > &input_x, const pcl::PointCloud< PointInT > &input_y, pcl::PointCloud< PointOutT > &output)
Perform Canny edge detection with two separated input images for horizontal and vertical derivatives...
Definition: edge.hpp:403
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities)
PointCloud represents the base class in PCL for storing collections of 3D points. ...
OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals, and OrganizedEdgeFromRGBNormals fi...
void assignLabelIndices(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Assign point indices for each edge label.
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:447
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) ...
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities) and assign point indices for each ed...
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) and assig...
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.
Definition: edge.hpp:329