Point Cloud Library (PCL)  1.14.1-dev
edge.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 #pragma once
39 
40 #include <pcl/2d/convolution.h>
41 #include <pcl/2d/edge.h>
42 #include <pcl/common/angles.h> // for rad2deg
43 
44 namespace pcl {
45 
46 template <typename PointInT, typename PointOutT>
47 void
48 Edge<PointInT, PointOutT>::detectEdgeSobel(pcl::PointCloud<PointOutT>& output)
49 {
50  convolution_.setInputCloud(input_);
51  pcl::PointCloud<PointXYZI>::Ptr kernel_x(new pcl::PointCloud<PointXYZI>);
52  pcl::PointCloud<PointXYZI>::Ptr magnitude_x(new pcl::PointCloud<PointXYZI>);
53  kernel_.setKernelType(kernel<PointXYZI>::SOBEL_X);
54  kernel_.fetchKernel(*kernel_x);
55  convolution_.setKernel(*kernel_x);
56  convolution_.filter(*magnitude_x);
57 
58  pcl::PointCloud<PointXYZI>::Ptr kernel_y(new pcl::PointCloud<PointXYZI>);
59  pcl::PointCloud<PointXYZI>::Ptr magnitude_y(new pcl::PointCloud<PointXYZI>);
60  kernel_.setKernelType(kernel<PointXYZI>::SOBEL_Y);
61  kernel_.fetchKernel(*kernel_y);
62  convolution_.setKernel(*kernel_y);
63  convolution_.filter(*magnitude_y);
64 
65  const int height = input_->height;
66  const int width = input_->width;
67 
68  output.resize(height * width);
69  output.height = height;
70  output.width = width;
71 
72  for (std::size_t i = 0; i < output.size(); ++i) {
73  output[i].magnitude_x = (*magnitude_x)[i].intensity;
74  output[i].magnitude_y = (*magnitude_y)[i].intensity;
75  output[i].magnitude =
76  std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
77  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
78  output[i].direction =
79  std::atan2((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
80  }
81 }
82 
83 template <typename PointInT, typename PointOutT>
84 void
85 Edge<PointInT, PointOutT>::sobelMagnitudeDirection(
86  const pcl::PointCloud<PointInT>& input_x,
87  const pcl::PointCloud<PointInT>& input_y,
88  pcl::PointCloud<PointOutT>& output)
89 {
90  convolution_.setInputCloud(input_x.makeShared());
91  pcl::PointCloud<PointXYZI>::Ptr kernel_x(new pcl::PointCloud<PointXYZI>);
92  pcl::PointCloud<PointXYZI>::Ptr magnitude_x(new pcl::PointCloud<PointXYZI>);
93  kernel_.setKernelType(kernel<PointXYZI>::SOBEL_X);
94  kernel_.fetchKernel(*kernel_x);
95  convolution_.setKernel(*kernel_x);
96  convolution_.filter(*magnitude_x);
97 
98  convolution_.setInputCloud(input_y.makeShared());
99  pcl::PointCloud<PointXYZI>::Ptr kernel_y(new pcl::PointCloud<PointXYZI>);
100  pcl::PointCloud<PointXYZI>::Ptr magnitude_y(new pcl::PointCloud<PointXYZI>);
101  kernel_.setKernelType(kernel<PointXYZI>::SOBEL_Y);
102  kernel_.fetchKernel(*kernel_y);
103  convolution_.setKernel(*kernel_y);
104  convolution_.filter(*magnitude_y);
105 
106  const int height = input_x.height;
107  const int width = input_x.width;
108 
109  output.resize(height * width);
110  output.height = height;
111  output.width = width;
112 
113  for (std::size_t i = 0; i < output.size(); ++i) {
114  output[i].magnitude_x = (*magnitude_x)[i].intensity;
115  output[i].magnitude_y = (*magnitude_y)[i].intensity;
116  output[i].magnitude =
117  std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
118  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
119  output[i].direction =
120  std::atan2((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
121  }
122 }
123 
124 template <typename PointInT, typename PointOutT>
125 void
126 Edge<PointInT, PointOutT>::detectEdgePrewitt(pcl::PointCloud<PointOutT>& output)
127 {
128  convolution_.setInputCloud(input_);
129 
130  pcl::PointCloud<PointXYZI>::Ptr kernel_x(new pcl::PointCloud<PointXYZI>);
131  pcl::PointCloud<PointXYZI>::Ptr magnitude_x(new pcl::PointCloud<PointXYZI>);
132  kernel_.setKernelType(kernel<PointXYZI>::PREWITT_X);
133  kernel_.fetchKernel(*kernel_x);
134  convolution_.setKernel(*kernel_x);
135  convolution_.filter(*magnitude_x);
136 
137  pcl::PointCloud<PointXYZI>::Ptr kernel_y(new pcl::PointCloud<PointXYZI>);
138  pcl::PointCloud<PointXYZI>::Ptr magnitude_y(new pcl::PointCloud<PointXYZI>);
139  kernel_.setKernelType(kernel<PointXYZI>::PREWITT_Y);
140  kernel_.fetchKernel(*kernel_y);
141  convolution_.setKernel(*kernel_y);
142  convolution_.filter(*magnitude_y);
143 
144  const int height = input_->height;
145  const int width = input_->width;
146 
147  output.resize(height * width);
148  output.height = height;
149  output.width = width;
150 
151  for (std::size_t i = 0; i < output.size(); ++i) {
152  output[i].magnitude_x = (*magnitude_x)[i].intensity;
153  output[i].magnitude_y = (*magnitude_y)[i].intensity;
154  output[i].magnitude =
155  std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
156  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
157  output[i].direction =
158  std::atan2((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
159  }
160 }
161 
162 template <typename PointInT, typename PointOutT>
163 void
164 Edge<PointInT, PointOutT>::detectEdgeRoberts(pcl::PointCloud<PointOutT>& output)
165 {
166  convolution_.setInputCloud(input_);
167 
168  pcl::PointCloud<PointXYZI>::Ptr kernel_x(new pcl::PointCloud<PointXYZI>);
169  pcl::PointCloud<PointXYZI>::Ptr magnitude_x(new pcl::PointCloud<PointXYZI>);
170  kernel_.setKernelType(kernel<PointXYZI>::ROBERTS_X);
171  kernel_.fetchKernel(*kernel_x);
172  convolution_.setKernel(*kernel_x);
173  convolution_.filter(*magnitude_x);
174 
175  pcl::PointCloud<PointXYZI>::Ptr kernel_y(new pcl::PointCloud<PointXYZI>);
176  pcl::PointCloud<PointXYZI>::Ptr magnitude_y(new pcl::PointCloud<PointXYZI>);
177  kernel_.setKernelType(kernel<PointXYZI>::ROBERTS_Y);
178  kernel_.fetchKernel(*kernel_y);
179  convolution_.setKernel(*kernel_y);
180  convolution_.filter(*magnitude_y);
181 
182  const int height = input_->height;
183  const int width = input_->width;
184 
185  output.resize(height * width);
186  output.height = height;
187  output.width = width;
188 
189  for (std::size_t i = 0; i < output.size(); ++i) {
190  output[i].magnitude_x = (*magnitude_x)[i].intensity;
191  output[i].magnitude_y = (*magnitude_y)[i].intensity;
192  output[i].magnitude =
193  std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
194  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
195  output[i].direction =
196  std::atan2((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
197  }
198 }
199 
200 template <typename PointInT, typename PointOutT>
201 void
202 Edge<PointInT, PointOutT>::cannyTraceEdge(
203  int rowOffset, int colOffset, int row, int col, pcl::PointCloud<PointXYZI>& maxima)
204 {
205  int newRow = row + rowOffset;
206  int newCol = col + colOffset;
207  PointXYZI& pt = maxima(newCol, newRow);
208 
209  if (newRow > 0 && newRow < static_cast<int>(maxima.height) && newCol > 0 &&
210  newCol < static_cast<int>(maxima.width)) {
211  if (pt.intensity == 0.0f || pt.intensity == std::numeric_limits<float>::max())
212  return;
213 
214  pt.intensity = std::numeric_limits<float>::max();
215  cannyTraceEdge(1, 0, newRow, newCol, maxima);
216  cannyTraceEdge(-1, 0, newRow, newCol, maxima);
217  cannyTraceEdge(1, 1, newRow, newCol, maxima);
218  cannyTraceEdge(-1, -1, newRow, newCol, maxima);
219  cannyTraceEdge(0, -1, newRow, newCol, maxima);
220  cannyTraceEdge(0, 1, newRow, newCol, maxima);
221  cannyTraceEdge(-1, 1, newRow, newCol, maxima);
222  cannyTraceEdge(1, -1, newRow, newCol, maxima);
223  }
224 }
225 
226 template <typename PointInT, typename PointOutT>
227 void
228 Edge<PointInT, PointOutT>::discretizeAngles(pcl::PointCloud<PointOutT>& thet)
229 {
230  const int height = thet.height;
231  const int width = thet.width;
232  float angle;
233  for (int i = 0; i < height; i++) {
234  for (int j = 0; j < width; j++) {
235  angle = pcl::rad2deg(thet(j, i).direction);
236  if (((angle <= 22.5) && (angle >= -22.5)) || (angle >= 157.5) ||
237  (angle <= -157.5))
238  thet(j, i).direction = 0;
239  else if (((angle > 22.5) && (angle < 67.5)) ||
240  ((angle < -112.5) && (angle > -157.5)))
241  thet(j, i).direction = 45;
242  else if (((angle >= 67.5) && (angle <= 112.5)) ||
243  ((angle <= -67.5) && (angle >= -112.5)))
244  thet(j, i).direction = 90;
245  else if (((angle > 112.5) && (angle < 157.5)) ||
246  ((angle < -22.5) && (angle > -67.5)))
247  thet(j, i).direction = 135;
248  }
249  }
250 }
251 
252 template <typename PointInT, typename PointOutT>
253 void
254 Edge<PointInT, PointOutT>::suppressNonMaxima(
255  const pcl::PointCloud<PointXYZIEdge>& edges,
256  pcl::PointCloud<PointXYZI>& maxima,
257  float tLow)
258 {
259  const int height = edges.height;
260  const int width = edges.width;
261 
262  maxima.height = height;
263  maxima.width = width;
264  maxima.resize(height * width);
265 
266  for (auto& point : maxima)
267  point.intensity = 0.0f;
268 
269  // tHigh and non-maximal suppression
270  for (int i = 1; i < height - 1; i++) {
271  for (int j = 1; j < width - 1; j++) {
272  const PointXYZIEdge& ptedge = edges(j, i);
273  PointXYZI& ptmax = maxima(j, i);
274 
275  if (ptedge.magnitude < tLow)
276  continue;
277 
278  // maxima (j, i).intensity = 0;
279 
280  switch (static_cast<int>(ptedge.direction)) {
281  case 0: {
282  if (ptedge.magnitude >= edges(j - 1, i).magnitude &&
283  ptedge.magnitude >= edges(j + 1, i).magnitude)
284  ptmax.intensity = ptedge.magnitude;
285  break;
286  }
287  case 45: {
288  if (ptedge.magnitude >= edges(j - 1, i - 1).magnitude &&
289  ptedge.magnitude >= edges(j + 1, i + 1).magnitude)
290  ptmax.intensity = ptedge.magnitude;
291  break;
292  }
293  case 90: {
294  if (ptedge.magnitude >= edges(j, i - 1).magnitude &&
295  ptedge.magnitude >= edges(j, i + 1).magnitude)
296  ptmax.intensity = ptedge.magnitude;
297  break;
298  }
299  case 135: {
300  if (ptedge.magnitude >= edges(j + 1, i - 1).magnitude &&
301  ptedge.magnitude >= edges(j - 1, i + 1).magnitude)
302  ptmax.intensity = ptedge.magnitude;
303  break;
304  }
305  }
306  }
307  }
308 }
309 
310 template <typename PointInT, typename PointOutT>
311 void
312 Edge<PointInT, PointOutT>::detectEdgeCanny(pcl::PointCloud<PointOutT>& output)
313 {
314  float tHigh = hysteresis_threshold_high_;
315  float tLow = hysteresis_threshold_low_;
316  const int height = input_->height;
317  const int width = input_->width;
318 
319  output.resize(height * width);
320  output.height = height;
321  output.width = width;
322 
323  // Noise reduction using gaussian blurring
324  pcl::PointCloud<PointXYZI>::Ptr gaussian_kernel(new pcl::PointCloud<PointXYZI>);
325  PointCloudInPtr smoothed_cloud(new PointCloudIn);
326  kernel_.setKernelSize(3);
327  kernel_.setKernelSigma(1.0);
328  kernel_.setKernelType(kernel<PointXYZI>::GAUSSIAN);
329  kernel_.fetchKernel(*gaussian_kernel);
330  convolution_.setKernel(*gaussian_kernel);
331  convolution_.setInputCloud(input_);
332  convolution_.filter(*smoothed_cloud);
333 
334  // Edge detection using Sobel
335  pcl::PointCloud<PointXYZIEdge>::Ptr edges(new pcl::PointCloud<PointXYZIEdge>);
336  setInputCloud(smoothed_cloud);
337  detectEdgeSobel(*edges);
338 
339  // Edge discretization
340  discretizeAngles(*edges);
341 
342  // tHigh and non-maximal suppression
343  pcl::PointCloud<PointXYZI>::Ptr maxima(new pcl::PointCloud<PointXYZI>);
344  suppressNonMaxima(*edges, *maxima, tLow);
345 
346  // Edge tracing
347  for (int i = 0; i < height; i++) {
348  for (int j = 0; j < width; j++) {
349  if ((*maxima)(j, i).intensity < tHigh ||
350  (*maxima)(j, i).intensity == std::numeric_limits<float>::max())
351  continue;
352 
353  (*maxima)(j, i).intensity = std::numeric_limits<float>::max();
354  cannyTraceEdge(1, 0, i, j, *maxima);
355  cannyTraceEdge(-1, 0, i, j, *maxima);
356  cannyTraceEdge(1, 1, i, j, *maxima);
357  cannyTraceEdge(-1, -1, i, j, *maxima);
358  cannyTraceEdge(0, -1, i, j, *maxima);
359  cannyTraceEdge(0, 1, i, j, *maxima);
360  cannyTraceEdge(-1, 1, i, j, *maxima);
361  cannyTraceEdge(1, -1, i, j, *maxima);
362  }
363  }
364 
365  // Final thresholding
366  for (std::size_t i = 0; i < input_->size(); ++i) {
367  if ((*maxima)[i].intensity == std::numeric_limits<float>::max())
368  output[i].magnitude = 255;
369  else
370  output[i].magnitude = 0;
371  }
372 }
373 
374 template <typename PointInT, typename PointOutT>
375 void
376 Edge<PointInT, PointOutT>::canny(const pcl::PointCloud<PointInT>& input_x,
377  const pcl::PointCloud<PointInT>& input_y,
378  pcl::PointCloud<PointOutT>& output)
379 {
380  float tHigh = hysteresis_threshold_high_;
381  float tLow = hysteresis_threshold_low_;
382  const int height = input_x.height;
383  const int width = input_x.width;
384 
385  output.resize(height * width);
386  output.height = height;
387  output.width = width;
388 
389  // Noise reduction using gaussian blurring
390  pcl::PointCloud<PointXYZI>::Ptr gaussian_kernel(new pcl::PointCloud<PointXYZI>);
391  kernel_.setKernelSize(3);
392  kernel_.setKernelSigma(1.0);
393  kernel_.setKernelType(kernel<PointXYZI>::GAUSSIAN);
394  kernel_.fetchKernel(*gaussian_kernel);
395  convolution_.setKernel(*gaussian_kernel);
396 
397  PointCloudIn smoothed_cloud_x;
398  convolution_.setInputCloud(input_x.makeShared());
399  convolution_.filter(smoothed_cloud_x);
400 
401  PointCloudIn smoothed_cloud_y;
402  convolution_.setInputCloud(input_y.makeShared());
403  convolution_.filter(smoothed_cloud_y);
404 
405  // Edge detection using Sobel
406  pcl::PointCloud<PointXYZIEdge>::Ptr edges(new pcl::PointCloud<PointXYZIEdge>);
407  sobelMagnitudeDirection(smoothed_cloud_x, smoothed_cloud_y, *edges.get());
408 
409  // Edge discretization
410  discretizeAngles(*edges);
411 
412  pcl::PointCloud<PointXYZI>::Ptr maxima(new pcl::PointCloud<PointXYZI>);
413  suppressNonMaxima(*edges, *maxima, tLow);
414 
415  // Edge tracing
416  for (int i = 0; i < height; i++) {
417  for (int j = 0; j < width; j++) {
418  if ((*maxima)(j, i).intensity < tHigh ||
419  (*maxima)(j, i).intensity == std::numeric_limits<float>::max())
420  continue;
421 
422  (*maxima)(j, i).intensity = std::numeric_limits<float>::max();
423 
424  // clang-format off
425  cannyTraceEdge( 1, 0, i, j, *maxima);
426  cannyTraceEdge(-1, 0, i, j, *maxima);
427  cannyTraceEdge( 1, 1, i, j, *maxima);
428  cannyTraceEdge(-1, -1, i, j, *maxima);
429  cannyTraceEdge( 0, -1, i, j, *maxima);
430  cannyTraceEdge( 0, 1, i, j, *maxima);
431  cannyTraceEdge(-1, 1, i, j, *maxima);
432  cannyTraceEdge( 1, -1, i, j, *maxima);
433  // clang-format on
434  }
435  }
436 
437  // Final thresholding
438  for (int i = 0; i < height; i++) {
439  for (int j = 0; j < width; j++) {
440  if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max())
441  output(j, i).magnitude = 255;
442  else
443  output(j, i).magnitude = 0;
444  }
445  }
446 }
447 
448 template <typename PointInT, typename PointOutT>
449 void
450 Edge<PointInT, PointOutT>::detectEdgeLoG(const float kernel_sigma,
451  const float kernel_size,
452  pcl::PointCloud<PointOutT>& output)
453 {
454  convolution_.setInputCloud(input_);
455 
456  pcl::PointCloud<PointXYZI>::Ptr log_kernel(new pcl::PointCloud<PointXYZI>);
457  kernel_.setKernelType(kernel<PointXYZI>::LOG);
458  kernel_.setKernelSigma(kernel_sigma);
459  kernel_.setKernelSize(kernel_size);
460  kernel_.fetchKernel(*log_kernel);
461  convolution_.setKernel(*log_kernel);
462  convolution_.filter(output);
463 }
464 
465 } // namespace pcl
angles.h
Define standard C methods to do angle calculations.
pcl::Edge::detectEdgePrewitt
void detectEdgePrewitt(pcl::PointCloud< PointOutT > &output)
Uses the Prewitt kernel for edge detection.
Definition: edge.hpp:126
pcl::Edge::detectEdgeLoG
void detectEdgeLoG(const float kernel_sigma, const float kernel_size, pcl::PointCloud< PointOutT > &output)
Uses the LoG kernel for edge detection.
Definition: edge.hpp:450
pcl::Edge::detectEdgeSobel
void detectEdgeSobel(pcl::PointCloud< PointOutT > &output)
Uses the Sobel kernel for edge detection.
Definition: edge.hpp:48
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::Edge::sobelMagnitudeDirection
void sobelMagnitudeDirection(const pcl::PointCloud< PointInT > &input_x, const pcl::PointCloud< PointInT > &input_y, pcl::PointCloud< PointOutT > &output)
Definition: edge.hpp:85
pcl::Edge::detectEdgeRoberts
void detectEdgeRoberts(pcl::PointCloud< PointOutT > &output)
Uses the Roberts kernel for edge detection.
Definition: edge.hpp:164
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::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
pcl::PointCloud::makeShared
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed,...
Definition: point_cloud.h:898
pcl::rad2deg
float rad2deg(float alpha)
Convert an angle from radians to degrees.
Definition: angles.hpp:61
pcl::_PointXYZI::intensity
float intensity
Definition: point_types.hpp:477