Point Cloud Library (PCL)  1.12.1-dev
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 
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
52 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
67 pcl::OrganizedEdgeBase<PointT, PointLT>::assignLabelIndices (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
68 {
69  const auto 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
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
226 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 
235  extractEdges (labels);
236 
237  this->assignLabelIndices (labels, label_indices);
238 }
239 
240 //////////////////////////////////////////////////////////////////////////////
241 template<typename PointT, typename PointLT> void
243 {
244  if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY))
245  {
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 
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
274 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 
283  extractEdges (labels);
284 
285  this->assignLabelIndices (labels, label_indices);
286 }
287 
288 //////////////////////////////////////////////////////////////////////////////
289 template<typename PointT, typename PointNT, typename PointLT> void
291 {
292  if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE))
293  {
294 
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 
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
332 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 
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_
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:173
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
std::uint32_t label