Point Cloud Library (PCL)  1.8.1
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 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, RGB Canny edge, and high curvature r...
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
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities)
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 extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) ...
uint32_t label
void setHysteresisThresholdLow(float threshold)
Definition: edge.h:147
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
PointCloud represents the base class in PCL for storing collections of 3D points. ...
OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals, and OrganizedEdgeFromRGBNormals fi...
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 extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) ...
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 ...
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
void assignLabelIndices(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Assign point indices for each edge label.
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.
Definition: edge.hpp:329