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