Point Cloud Library (PCL)  1.11.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 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
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 && 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 && 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_
pcl::OrganizedEdgeFromNormals
Definition: organized_edge_detection.h:269
pcl::Label::label
std::uint32_t label
Definition: point_types.hpp:517
pcl::OrganizedEdgeBase::Neighbor
Definition: organized_edge_detection.h:157
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::Edge::setInputCloud
void setInputCloud(PointCloudInPtr input)
Set the input point cloud pointer.
Definition: edge.h:304
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::OrganizedEdgeBase::assignLabelIndices
void assignLabelIndices(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Assign point indices for each edge label.
Definition: organized_edge_detection.hpp:67
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::OrganizedEdgeFromNormals::compute
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...
Definition: organized_edge_detection.hpp:274
pcl::Label
Definition: point_types.hpp:515
pcl::Edge::canny
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
pcl::OrganizedEdgeBase::compute
void compute(pcl::PointCloud< PointLT > &labels, std::vector< pcl::PointIndices > &label_indices) const
Perform the 3D edge detection (edges from depth discontinuities)
Definition: organized_edge_detection.hpp:52
pcl::Edge::setHysteresisThresholdLow
void setHysteresisThresholdLow(float threshold)
Definition: edge.h:149
pcl::OrganizedEdgeFromRGB::compute
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 ...
Definition: organized_edge_detection.hpp:226
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::OrganizedEdgeBase
OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals, and OrganizedEdgeFromRGBNormals fi...
Definition: organized_edge_detection.h:57
pcl::OrganizedEdgeBase::Neighbor::d_index
int d_index
Definition: organized_edge_detection.h:167
pcl::OrganizedEdgeBase::extractEdges
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities) and assign point indices for each ed...
Definition: organized_edge_detection.hpp:86
pcl::OrganizedEdgeFromRGB::extractEdges
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge)
Definition: organized_edge_detection.hpp:242
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:407
pcl::OrganizedEdgeFromNormals::extractEdges
void extractEdges(pcl::PointCloud< PointLT > &labels) const
Perform the 3D edge detection (edges from depth discontinuities and high curvature regions)
Definition: organized_edge_detection.hpp:290
pcl::Edge::setHysteresisThresholdHigh
void setHysteresisThresholdHigh(float threshold)
Definition: edge.h:155
pcl::OrganizedEdgeFromRGB
Definition: organized_edge_detection.h:183
pcl::Edge::detectEdgeCanny
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.
Definition: edge.hpp:312
pcl::Edge
Definition: edge.h:49
pcl::OrganizedEdgeFromRGBNormals::compute
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...
Definition: organized_edge_detection.hpp:332