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> 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  {
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  atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
80  }
81 }
82 
83 //////////////////////////////////////////////////////////////////////////////
84 template <typename PointInT, typename PointOutT> 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  {
115  output[i].magnitude_x = (*magnitude_x)[i].intensity;
116  output[i].magnitude_y = (*magnitude_y)[i].intensity;
117  output[i].magnitude =
118  std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
119  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
120  output[i].direction =
121  atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
122  }
123 }
124 
125 //////////////////////////////////////////////////////////////////////////////
126 template <typename PointInT, typename PointOutT> 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  {
154  output[i].magnitude_x = (*magnitude_x)[i].intensity;
155  output[i].magnitude_y = (*magnitude_y)[i].intensity;
156  output[i].magnitude =
157  std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
158  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
159  output[i].direction =
160  atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
161  }
162 }
163 
164 //////////////////////////////////////////////////////////////////////////////
165 template <typename PointInT, typename PointOutT> 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  {
193  output[i].magnitude_x = (*magnitude_x)[i].intensity;
194  output[i].magnitude_y = (*magnitude_y)[i].intensity;
195  output[i].magnitude =
196  std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
197  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
198  output[i].direction =
199  atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
200  }
201 }
202 
203 //////////////////////////////////////////////////////////////////////////////
204 template<typename PointInT, typename PointOutT> void
206  int rowOffset, int colOffset, int row, int col,
208 {
209  int newRow = row + rowOffset;
210  int newCol = col + colOffset;
211  PointXYZI &pt = maxima (newCol, newRow);
212 
213  if (newRow > 0 && newRow < static_cast<int> (maxima.height) && newCol > 0 && newCol < static_cast<int> (maxima.width))
214  {
215  if (pt.intensity == 0.0f || pt.intensity == std::numeric_limits<float>::max ())
216  return;
217 
218  pt.intensity = std::numeric_limits<float>::max ();
219  cannyTraceEdge ( 1, 0, newRow, newCol, maxima);
220  cannyTraceEdge (-1, 0, newRow, newCol, maxima);
221  cannyTraceEdge ( 1, 1, newRow, newCol, maxima);
222  cannyTraceEdge (-1, -1, newRow, newCol, maxima);
223  cannyTraceEdge ( 0, -1, newRow, newCol, maxima);
224  cannyTraceEdge ( 0, 1, newRow, newCol, maxima);
225  cannyTraceEdge (-1, 1, newRow, newCol, maxima);
226  cannyTraceEdge ( 1, -1, newRow, newCol, maxima);
227  }
228 }
229 
230 //////////////////////////////////////////////////////////////////////////////
231 template <typename PointInT, typename PointOutT> 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  {
239  for (int j = 0; j < width; j++)
240  {
241  angle = pcl::rad2deg (thet (j, i).direction);
242  if (((angle <= 22.5) && (angle >= -22.5)) || (angle >= 157.5) || (angle <= -157.5))
243  thet (j, i).direction = 0;
244  else
245  if (((angle > 22.5) && (angle < 67.5)) || ((angle < -112.5) && (angle > -157.5)))
246  thet (j, i).direction = 45;
247  else
248  if (((angle >= 67.5) && (angle <= 112.5)) || ((angle <= -67.5) && (angle >= -112.5)))
249  thet (j, i).direction = 90;
250  else
251  if (((angle > 112.5) && (angle < 157.5)) || ((angle < -22.5) && (angle > -67.5)))
252  thet (j, i).direction = 135;
253  }
254  }
255 }
256 
257 //////////////////////////////////////////////////////////////////////////////
258 template <typename PointInT, typename PointOutT> void
260  const pcl::PointCloud<PointXYZIEdge> &edges,
261  pcl::PointCloud<PointXYZI> &maxima, float tLow)
262 {
263  const int height = edges.height;
264  const int width = edges.width;
265 
266  maxima.height = height;
267  maxima.width = width;
268  maxima.resize (height * width);
269 
270  for (auto &point : maxima)
271  point.intensity = 0.0f;
272 
273  // tHigh and non-maximal supression
274  for (int i = 1; i < height - 1; i++)
275  {
276  for (int j = 1; j < width - 1; j++)
277  {
278  const PointXYZIEdge &ptedge = edges (j, i);
279  PointXYZI &ptmax = maxima (j, i);
280 
281  if (ptedge.magnitude < tLow)
282  continue;
283 
284  //maxima (j, i).intensity = 0;
285 
286  switch (int (ptedge.direction))
287  {
288  case 0:
289  {
290  if (ptedge.magnitude >= edges (j - 1, i).magnitude &&
291  ptedge.magnitude >= edges (j + 1, i).magnitude)
292  ptmax.intensity = ptedge.magnitude;
293  break;
294  }
295  case 45:
296  {
297  if (ptedge.magnitude >= edges (j - 1, i - 1).magnitude &&
298  ptedge.magnitude >= edges (j + 1, i + 1).magnitude)
299  ptmax.intensity = ptedge.magnitude;
300  break;
301  }
302  case 90:
303  {
304  if (ptedge.magnitude >= edges (j, i - 1).magnitude &&
305  ptedge.magnitude >= edges (j, i + 1).magnitude)
306  ptmax.intensity = ptedge.magnitude;
307  break;
308  }
309  case 135:
310  {
311  if (ptedge.magnitude >= edges (j + 1, i - 1).magnitude &&
312  ptedge.magnitude >= edges (j - 1, i + 1).magnitude)
313  ptmax.intensity = ptedge.magnitude;
314  break;
315  }
316  }
317  }
318  }
319 }
320 
321 //////////////////////////////////////////////////////////////////////////////
322 template<typename PointInT, typename PointOutT> void
324 {
325  float tHigh = hysteresis_threshold_high_;
326  float tLow = hysteresis_threshold_low_;
327  const int height = input_->height;
328  const int width = input_->width;
329 
330  output.resize (height * width);
331  output.height = height;
332  output.width = width;
333 
334  // Noise reduction using gaussian blurring
336  PointCloudInPtr smoothed_cloud (new PointCloudIn);
337  kernel_.setKernelSize (3);
338  kernel_.setKernelSigma (1.0);
339  kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
340  kernel_.fetchKernel (*gaussian_kernel);
341  convolution_.setKernel (*gaussian_kernel);
342  convolution_.setInputCloud (input_);
343  convolution_.filter (*smoothed_cloud);
344 
345  // Edge detection using Sobel
347  setInputCloud (smoothed_cloud);
348  detectEdgeSobel (*edges);
349 
350  // Edge discretization
351  discretizeAngles (*edges);
352 
353  // tHigh and non-maximal supression
355  suppressNonMaxima (*edges, *maxima, tLow);
356 
357  // Edge tracing
358  for (int i = 0; i < height; i++)
359  {
360  for (int j = 0; j < width; j++)
361  {
362  if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
363  continue;
364 
365  (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
366  cannyTraceEdge ( 1, 0, i, j, *maxima);
367  cannyTraceEdge (-1, 0, i, j, *maxima);
368  cannyTraceEdge ( 1, 1, i, j, *maxima);
369  cannyTraceEdge (-1, -1, i, j, *maxima);
370  cannyTraceEdge ( 0, -1, i, j, *maxima);
371  cannyTraceEdge ( 0, 1, i, j, *maxima);
372  cannyTraceEdge (-1, 1, i, j, *maxima);
373  cannyTraceEdge ( 1, -1, i, j, *maxima);
374  }
375  }
376 
377  // Final thresholding
378  for (size_t i = 0; i < input_->size (); ++i)
379  {
380  if ((*maxima)[i].intensity == std::numeric_limits<float>::max ())
381  output[i].magnitude = 255;
382  else
383  output[i].magnitude = 0;
384  }
385 }
386 
387 //////////////////////////////////////////////////////////////////////////////
388 template <typename PointInT, typename PointOutT> void
390  const pcl::PointCloud<PointInT> &input_x,
391  const pcl::PointCloud<PointInT> &input_y,
393 {
394  float tHigh = hysteresis_threshold_high_;
395  float tLow = hysteresis_threshold_low_;
396  const int height = input_x.height;
397  const int width = input_x.width;
398 
399  output.resize (height * width);
400  output.height = height;
401  output.width = width;
402 
403  // Noise reduction using gaussian blurring
405  kernel_.setKernelSize (3);
406  kernel_.setKernelSigma (1.0);
407  kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
408  kernel_.fetchKernel (*gaussian_kernel);
409  convolution_.setKernel (*gaussian_kernel);
410 
411  PointCloudIn smoothed_cloud_x;
412  convolution_.setInputCloud (input_x.makeShared());
413  convolution_.filter (smoothed_cloud_x);
414 
415  PointCloudIn smoothed_cloud_y;
416  convolution_.setInputCloud (input_y.makeShared());
417  convolution_.filter (smoothed_cloud_y);
418 
419 
420  // Edge detection using Sobel
422  sobelMagnitudeDirection (smoothed_cloud_x, smoothed_cloud_y, *edges.get ());
423 
424  // Edge discretization
425  discretizeAngles (*edges);
426 
428  suppressNonMaxima (*edges, *maxima, tLow);
429 
430  // Edge tracing
431  for (int i = 0; i < height; i++)
432  {
433  for (int j = 0; j < width; j++)
434  {
435  if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
436  continue;
437 
438  (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
439  cannyTraceEdge ( 1, 0, i, j, *maxima);
440  cannyTraceEdge (-1, 0, i, j, *maxima);
441  cannyTraceEdge ( 1, 1, i, j, *maxima);
442  cannyTraceEdge (-1, -1, i, j, *maxima);
443  cannyTraceEdge ( 0, -1, i, j, *maxima);
444  cannyTraceEdge ( 0, 1, i, j, *maxima);
445  cannyTraceEdge (-1, 1, i, j, *maxima);
446  cannyTraceEdge ( 1, -1, i, j, *maxima);
447  }
448  }
449 
450  // Final thresholding
451  for (int i = 0; i < height; i++)
452  {
453  for (int j = 0; j < width; j++)
454  {
455  if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
456  output (j, i).magnitude = 255;
457  else
458  output (j, i).magnitude = 0;
459  }
460  }
461 }
462 
463 //////////////////////////////////////////////////////////////////////////////
464 template<typename PointInT, typename PointOutT> void
466  const float kernel_sigma, const float kernel_size,
468 {
469  convolution_.setInputCloud (input_);
470 
472  kernel_.setKernelType (kernel<PointXYZI>::LOG);
473  kernel_.setKernelSigma (kernel_sigma);
474  kernel_.setKernelSize (kernel_size);
475  kernel_.fetchKernel (*log_kernel);
476  convolution_.setKernel (*log_kernel);
477  convolution_.filter (output);
478 }
479 
480 #endif
size_t size() const
Definition: point_cloud.h:447
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:588
This typedef is used to represent a point cloud containing edge information.
Definition: convolution.h:49
float rad2deg(float alpha)
Convert an angle from radians to degrees.
Definition: angles.hpp:61
Definition: edge.h:47
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:414
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
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:412
void detectEdgePrewitt(pcl::PointCloud< PointOutT > &output)
Uses the Prewitt kernel for edge detection.
Definition: edge.hpp:127
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:389
void detectEdgeSobel(pcl::PointCloud< PointOutT > &output)
Uses the Sobel kernel for edge detection.
Definition: edge.hpp:46
void detectEdgeLoG(const float kernel_sigma, const float kernel_size, pcl::PointCloud< PointOutT > &output)
Uses the LoG kernel for edge detection.
Definition: edge.hpp:465
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:454
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.
Definition: edge.hpp:323
void detectEdgeRoberts(pcl::PointCloud< PointOutT > &output)
Uses the Roberts kernel for edge detection.
Definition: edge.hpp:166