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