Point Cloud Library (PCL)  1.9.0-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 #include <pcl/console/time.h>
44 
45 //////////////////////////////////////////////////////////////////////////////
46 template <typename PointInT, typename PointOutT> void
49 {
50  //pcl::console::TicToc tt;
51  //tt.tic ();
52  convolution_.setInputCloud (input_);
55  kernel_.setKernelType (kernel<PointXYZI>::SOBEL_X);
56  kernel_.fetchKernel (*kernel_x);
57  convolution_.setKernel (*kernel_x);
58  convolution_.filter (*magnitude_x);
59  //PCL_ERROR ("Convolve X: %g\n", tt.toc ()); tt.tic ();
60 
63  kernel_.setKernelType (kernel<PointXYZI>::SOBEL_Y);
64  kernel_.fetchKernel (*kernel_y);
65  convolution_.setKernel (*kernel_y);
66  convolution_.filter (*magnitude_y);
67  //PCL_ERROR ("Convolve Y: %g\n", tt.toc ()); tt.tic ();
68 
69  const int height = input_->height;
70  const int width = input_->width;
71 
72  output.resize (height * width);
73  output.height = height;
74  output.width = width;
75 
76  for (size_t i = 0; i < output.size (); ++i)
77  {
78  output[i].magnitude_x = (*magnitude_x)[i].intensity;
79  output[i].magnitude_y = (*magnitude_y)[i].intensity;
80  output[i].magnitude =
81  std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
82  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
83  output[i].direction =
84  atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
85  }
86  //PCL_ERROR ("Rest: %g\n", tt.toc ());
87 }
88 
89 //////////////////////////////////////////////////////////////////////////////
90 template <typename PointInT, typename PointOutT> void
92  const pcl::PointCloud<PointInT> &input_x,
93  const pcl::PointCloud<PointInT> &input_y,
95 {
96  convolution_.setInputCloud (input_x.makeShared());
99  kernel_.setKernelType (kernel<PointXYZI>::SOBEL_X);
100  kernel_.fetchKernel (*kernel_x);
101  convolution_.setKernel (*kernel_x);
102  convolution_.filter (*magnitude_x);
103 
104  convolution_.setInputCloud (input_y.makeShared());
107  kernel_.setKernelType (kernel<PointXYZI>::SOBEL_Y);
108  kernel_.fetchKernel (*kernel_y);
109  convolution_.setKernel (*kernel_y);
110  convolution_.filter (*magnitude_y);
111 
112  const int height = input_x.height;
113  const int width = input_x.width;
114 
115  output.resize (height * width);
116  output.height = height;
117  output.width = width;
118 
119  for (size_t i = 0; i < output.size (); ++i)
120  {
121  output[i].magnitude_x = (*magnitude_x)[i].intensity;
122  output[i].magnitude_y = (*magnitude_y)[i].intensity;
123  output[i].magnitude =
124  std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
125  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
126  output[i].direction =
127  atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
128  }
129 }
130 
131 //////////////////////////////////////////////////////////////////////////////
132 template <typename PointInT, typename PointOutT> void
134 {
135  convolution_.setInputCloud (input_);
136 
139  kernel_.setKernelType (kernel<PointXYZI>::PREWITT_X);
140  kernel_.fetchKernel (*kernel_x);
141  convolution_.setKernel (*kernel_x);
142  convolution_.filter (*magnitude_x);
143 
146  kernel_.setKernelType (kernel<PointXYZI>::PREWITT_Y);
147  kernel_.fetchKernel (*kernel_y);
148  convolution_.setKernel (*kernel_y);
149  convolution_.filter (*magnitude_y);
150 
151  const int height = input_->height;
152  const int width = input_->width;
153 
154  output.resize (height * width);
155  output.height = height;
156  output.width = width;
157 
158  for (size_t i = 0; i < output.size (); ++i)
159  {
160  output[i].magnitude_x = (*magnitude_x)[i].intensity;
161  output[i].magnitude_y = (*magnitude_y)[i].intensity;
162  output[i].magnitude =
163  std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
164  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
165  output[i].direction =
166  atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
167  }
168 }
169 
170 //////////////////////////////////////////////////////////////////////////////
171 template <typename PointInT, typename PointOutT> void
173 {
174  convolution_.setInputCloud (input_);
175 
178  kernel_.setKernelType (kernel<PointXYZI>::ROBERTS_X);
179  kernel_.fetchKernel (*kernel_x);
180  convolution_.setKernel (*kernel_x);
181  convolution_.filter (*magnitude_x);
182 
185  kernel_.setKernelType (kernel<PointXYZI>::ROBERTS_Y);
186  kernel_.fetchKernel (*kernel_y);
187  convolution_.setKernel (*kernel_y);
188  convolution_.filter (*magnitude_y);
189 
190  const int height = input_->height;
191  const int width = input_->width;
192 
193  output.resize (height * width);
194  output.height = height;
195  output.width = width;
196 
197  for (size_t i = 0; i < output.size (); ++i)
198  {
199  output[i].magnitude_x = (*magnitude_x)[i].intensity;
200  output[i].magnitude_y = (*magnitude_y)[i].intensity;
201  output[i].magnitude =
202  std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
203  (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
204  output[i].direction =
205  atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
206  }
207 }
208 
209 //////////////////////////////////////////////////////////////////////////////
210 template<typename PointInT, typename PointOutT> void
212  int rowOffset, int colOffset, int row, int col,
214 {
215  int newRow = row + rowOffset;
216  int newCol = col + colOffset;
217  PointXYZI &pt = maxima (newCol, newRow);
218 
219  if (newRow > 0 && newRow < static_cast<int> (maxima.height) && newCol > 0 && newCol < static_cast<int> (maxima.width))
220  {
221  if (pt.intensity == 0.0f || pt.intensity == std::numeric_limits<float>::max ())
222  return;
223 
224  pt.intensity = std::numeric_limits<float>::max ();
225  cannyTraceEdge ( 1, 0, newRow, newCol, maxima);
226  cannyTraceEdge (-1, 0, newRow, newCol, maxima);
227  cannyTraceEdge ( 1, 1, newRow, newCol, maxima);
228  cannyTraceEdge (-1, -1, newRow, newCol, maxima);
229  cannyTraceEdge ( 0, -1, newRow, newCol, maxima);
230  cannyTraceEdge ( 0, 1, newRow, newCol, maxima);
231  cannyTraceEdge (-1, 1, newRow, newCol, maxima);
232  cannyTraceEdge ( 1, -1, newRow, newCol, maxima);
233  }
234 }
235 
236 //////////////////////////////////////////////////////////////////////////////
237 template <typename PointInT, typename PointOutT> void
239 {
240  const int height = thet.height;
241  const int width = thet.width;
242  float angle;
243  for (int i = 0; i < height; i++)
244  {
245  for (int j = 0; j < width; j++)
246  {
247  angle = pcl::rad2deg (thet (j, i).direction);
248  if (((angle <= 22.5) && (angle >= -22.5)) || (angle >= 157.5) || (angle <= -157.5))
249  thet (j, i).direction = 0;
250  else
251  if (((angle > 22.5) && (angle < 67.5)) || ((angle < -112.5) && (angle > -157.5)))
252  thet (j, i).direction = 45;
253  else
254  if (((angle >= 67.5) && (angle <= 112.5)) || ((angle <= -67.5) && (angle >= -112.5)))
255  thet (j, i).direction = 90;
256  else
257  if (((angle > 112.5) && (angle < 157.5)) || ((angle < -22.5) && (angle > -67.5)))
258  thet (j, i).direction = 135;
259  }
260  }
261 }
262 
263 //////////////////////////////////////////////////////////////////////////////
264 template <typename PointInT, typename PointOutT> void
266  const pcl::PointCloud<PointXYZIEdge> &edges,
267  pcl::PointCloud<PointXYZI> &maxima, float tLow)
268 {
269  const int height = edges.height;
270  const int width = edges.width;
271 
272  maxima.height = height;
273  maxima.width = width;
274  maxima.resize (height * width);
275 
276  for (size_t i = 0; i < maxima.size (); ++i)
277  maxima[i].intensity = 0.0f;
278 
279  // tHigh and non-maximal supression
280  for (int i = 1; i < height - 1; i++)
281  {
282  for (int j = 1; j < width - 1; j++)
283  {
284  const PointXYZIEdge &ptedge = edges (j, i);
285  PointXYZI &ptmax = maxima (j, i);
286 
287  if (ptedge.magnitude < tLow)
288  continue;
289 
290  //maxima (j, i).intensity = 0;
291 
292  switch (int (ptedge.direction))
293  {
294  case 0:
295  {
296  if (ptedge.magnitude >= edges (j - 1, i).magnitude &&
297  ptedge.magnitude >= edges (j + 1, i).magnitude)
298  ptmax.intensity = ptedge.magnitude;
299  break;
300  }
301  case 45:
302  {
303  if (ptedge.magnitude >= edges (j - 1, i - 1).magnitude &&
304  ptedge.magnitude >= edges (j + 1, i + 1).magnitude)
305  ptmax.intensity = ptedge.magnitude;
306  break;
307  }
308  case 90:
309  {
310  if (ptedge.magnitude >= edges (j, i - 1).magnitude &&
311  ptedge.magnitude >= edges (j, i + 1).magnitude)
312  ptmax.intensity = ptedge.magnitude;
313  break;
314  }
315  case 135:
316  {
317  if (ptedge.magnitude >= edges (j + 1, i - 1).magnitude &&
318  ptedge.magnitude >= edges (j - 1, i + 1).magnitude)
319  ptmax.intensity = ptedge.magnitude;
320  break;
321  }
322  }
323  }
324  }
325 }
326 
327 //////////////////////////////////////////////////////////////////////////////
328 template<typename PointInT, typename PointOutT> void
330 {
331  float tHigh = hysteresis_threshold_high_;
332  float tLow = hysteresis_threshold_low_;
333  const int height = input_->height;
334  const int width = input_->width;
335 
336  output.resize (height * width);
337  output.height = height;
338  output.width = width;
339 
340  //pcl::console::TicToc tt;
341  //tt.tic ();
342 
343  // Noise reduction using gaussian blurring
345  PointCloudInPtr smoothed_cloud (new PointCloudIn);
346  kernel_.setKernelSize (3);
347  kernel_.setKernelSigma (1.0);
348  kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
349  kernel_.fetchKernel (*gaussian_kernel);
350  convolution_.setKernel (*gaussian_kernel);
351  convolution_.setInputCloud (input_);
352  convolution_.filter (*smoothed_cloud);
353  //PCL_ERROR ("Gaussian blur: %g\n", tt.toc ()); tt.tic ();
354 
355  // Edge detection using Sobel
357  setInputCloud (smoothed_cloud);
358  detectEdgeSobel (*edges);
359  //PCL_ERROR ("Sobel: %g\n", tt.toc ()); tt.tic ();
360 
361  // Edge discretization
362  discretizeAngles (*edges);
363  //PCL_ERROR ("Discretize: %g\n", tt.toc ()); tt.tic ();
364 
365  // tHigh and non-maximal supression
367  suppressNonMaxima (*edges, *maxima, tLow);
368  //PCL_ERROR ("NM suppress: %g\n", tt.toc ()); tt.tic ();
369 
370  // Edge tracing
371  for (int i = 0; i < height; i++)
372  {
373  for (int j = 0; j < width; j++)
374  {
375  if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
376  continue;
377 
378  (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
379  cannyTraceEdge ( 1, 0, i, j, *maxima);
380  cannyTraceEdge (-1, 0, i, j, *maxima);
381  cannyTraceEdge ( 1, 1, i, j, *maxima);
382  cannyTraceEdge (-1, -1, i, j, *maxima);
383  cannyTraceEdge ( 0, -1, i, j, *maxima);
384  cannyTraceEdge ( 0, 1, i, j, *maxima);
385  cannyTraceEdge (-1, 1, i, j, *maxima);
386  cannyTraceEdge ( 1, -1, i, j, *maxima);
387  }
388  }
389  //PCL_ERROR ("Edge tracing: %g\n", tt.toc ());
390 
391  // Final thresholding
392  for (size_t i = 0; i < input_->size (); ++i)
393  {
394  if ((*maxima)[i].intensity == std::numeric_limits<float>::max ())
395  output[i].magnitude = 255;
396  else
397  output[i].magnitude = 0;
398  }
399 }
400 
401 //////////////////////////////////////////////////////////////////////////////
402 template <typename PointInT, typename PointOutT> void
404  const pcl::PointCloud<PointInT> &input_x,
405  const pcl::PointCloud<PointInT> &input_y,
407 {
408  float tHigh = hysteresis_threshold_high_;
409  float tLow = hysteresis_threshold_low_;
410  const int height = input_x.height;
411  const int width = input_x.width;
412 
413  output.resize (height * width);
414  output.height = height;
415  output.width = width;
416 
417  // Noise reduction using gaussian blurring
419  kernel_.setKernelSize (3);
420  kernel_.setKernelSigma (1.0);
421  kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
422  kernel_.fetchKernel (*gaussian_kernel);
423  convolution_.setKernel (*gaussian_kernel);
424 
425  PointCloudIn smoothed_cloud_x;
426  convolution_.setInputCloud (input_x.makeShared());
427  convolution_.filter (smoothed_cloud_x);
428 
429  PointCloudIn smoothed_cloud_y;
430  convolution_.setInputCloud (input_y.makeShared());
431  convolution_.filter (smoothed_cloud_y);
432 
433 
434  // Edge detection using Sobel
436  sobelMagnitudeDirection (smoothed_cloud_x, smoothed_cloud_y, *edges.get ());
437 
438  // Edge discretization
439  discretizeAngles (*edges);
440 
442  suppressNonMaxima (*edges, *maxima, tLow);
443 
444  // Edge tracing
445  for (int i = 0; i < height; i++)
446  {
447  for (int j = 0; j < width; j++)
448  {
449  if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
450  continue;
451 
452  (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
453  cannyTraceEdge ( 1, 0, i, j, *maxima);
454  cannyTraceEdge (-1, 0, i, j, *maxima);
455  cannyTraceEdge ( 1, 1, i, j, *maxima);
456  cannyTraceEdge (-1, -1, i, j, *maxima);
457  cannyTraceEdge ( 0, -1, i, j, *maxima);
458  cannyTraceEdge ( 0, 1, i, j, *maxima);
459  cannyTraceEdge (-1, 1, i, j, *maxima);
460  cannyTraceEdge ( 1, -1, i, j, *maxima);
461  }
462  }
463 
464  // Final thresholding
465  for (int i = 0; i < height; i++)
466  {
467  for (int j = 0; j < width; j++)
468  {
469  if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
470  output (j, i).magnitude = 255;
471  else
472  output (j, i).magnitude = 0;
473  }
474  }
475 }
476 
477 //////////////////////////////////////////////////////////////////////////////
478 template<typename PointInT, typename PointOutT> void
480  const float kernel_sigma, const float kernel_size,
482 {
483  convolution_.setInputCloud (input_);
484 
486  kernel_.setKernelType (kernel<PointXYZI>::LOG);
487  kernel_.setKernelSigma (kernel_sigma);
488  kernel_.setKernelSize (kernel_size);
489  kernel_.fetchKernel (*log_kernel);
490  convolution_.setKernel (*log_kernel);
491  convolution_.filter (output);
492 }
493 
494 #endif
size_t size() const
Definition: point_cloud.h:448
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:589
This typedef is used to represent a point cloud containing edge information.
Definition: convolution.h:50
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:415
boost::shared_ptr< PointCloud< PointT > > Ptr
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:91
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void detectEdgePrewitt(pcl::PointCloud< PointOutT > &output)
Uses the Prewitt kernel for edge detection.
Definition: edge.hpp:133
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:403
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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:479
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.
Definition: edge.hpp:329
void detectEdgeRoberts(pcl::PointCloud< PointOutT > &output)
Uses the Roberts kernel for edge detection.
Definition: edge.hpp:172