Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
44namespace pcl {
45
46template <typename PointInT, typename PointOutT>
47void
49{
50 convolution_.setInputCloud(input_);
53 kernel_.setKernelType(kernel<PointXYZI>::SOBEL_X);
54 kernel_.fetchKernel(*kernel_x);
55 convolution_.setKernel(*kernel_x);
56 convolution_.filter(*magnitude_x);
57
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
83template <typename PointInT, typename PointOutT>
84void
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 (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
124template <typename PointInT, typename PointOutT>
125void
127{
128 convolution_.setInputCloud(input_);
129
132 kernel_.setKernelType(kernel<PointXYZI>::PREWITT_X);
133 kernel_.fetchKernel(*kernel_x);
134 convolution_.setKernel(*kernel_x);
135 convolution_.filter(*magnitude_x);
136
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
162template <typename PointInT, typename PointOutT>
163void
165{
166 convolution_.setInputCloud(input_);
167
170 kernel_.setKernelType(kernel<PointXYZI>::ROBERTS_X);
171 kernel_.fetchKernel(*kernel_x);
172 convolution_.setKernel(*kernel_x);
173 convolution_.filter(*magnitude_x);
174
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
200template <typename PointInT, typename PointOutT>
201void
202Edge<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
226template <typename PointInT, typename PointOutT>
227void
228Edge<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
252template <typename PointInT, typename PointOutT>
253void
254Edge<PointInT, PointOutT>::suppressNonMaxima(
257 float tLow)
258{
259 const int height = edges.height;
260 const int width = edges.width;
261
262 maxima.resize(edges.width, edges.height);
263
264 for (auto& point : maxima)
265 point.intensity = 0.0f;
266
267 // tHigh and non-maximal suppression
268 for (int i = 1; i < height - 1; i++) {
269 for (int j = 1; j < width - 1; j++) {
270 const PointXYZIEdge& ptedge = edges(j, i);
271 PointXYZI& ptmax = maxima(j, i);
272
273 if (ptedge.magnitude < tLow)
274 continue;
275
276 // maxima (j, i).intensity = 0;
277
278 switch (static_cast<int>(ptedge.direction)) {
279 case 0: {
280 if (ptedge.magnitude >= edges(j - 1, i).magnitude &&
281 ptedge.magnitude >= edges(j + 1, i).magnitude)
282 ptmax.intensity = ptedge.magnitude;
283 break;
284 }
285 case 45: {
286 if (ptedge.magnitude >= edges(j - 1, i - 1).magnitude &&
287 ptedge.magnitude >= edges(j + 1, i + 1).magnitude)
288 ptmax.intensity = ptedge.magnitude;
289 break;
290 }
291 case 90: {
292 if (ptedge.magnitude >= edges(j, i - 1).magnitude &&
293 ptedge.magnitude >= edges(j, i + 1).magnitude)
294 ptmax.intensity = ptedge.magnitude;
295 break;
296 }
297 case 135: {
298 if (ptedge.magnitude >= edges(j + 1, i - 1).magnitude &&
299 ptedge.magnitude >= edges(j - 1, i + 1).magnitude)
300 ptmax.intensity = ptedge.magnitude;
301 break;
302 }
303 }
304 }
305 }
306}
307
308template <typename PointInT, typename PointOutT>
309void
311{
312 float tHigh = hysteresis_threshold_high_;
313 float tLow = hysteresis_threshold_low_;
314 const int height = input_->height;
315 const int width = input_->width;
316
317 output.resize(height * width);
318 output.height = height;
319 output.width = width;
320
321 // Noise reduction using gaussian blurring
323 PointCloudInPtr smoothed_cloud(new PointCloudIn);
324 kernel_.setKernelSize(3);
325 kernel_.setKernelSigma(1.0);
326 kernel_.setKernelType(kernel<PointXYZI>::GAUSSIAN);
327 kernel_.fetchKernel(*gaussian_kernel);
328 convolution_.setKernel(*gaussian_kernel);
329 convolution_.setInputCloud(input_);
330 convolution_.filter(*smoothed_cloud);
331
332 // Edge detection using Sobel
334 setInputCloud(smoothed_cloud);
335 detectEdgeSobel(*edges);
336
337 // Edge discretization
338 discretizeAngles(*edges);
339
340 // tHigh and non-maximal suppression
342 suppressNonMaxima(*edges, *maxima, tLow);
343
344 // Edge tracing
345 for (int i = 0; i < height; i++) {
346 for (int j = 0; j < width; j++) {
347 if ((*maxima)(j, i).intensity < tHigh ||
348 (*maxima)(j, i).intensity == std::numeric_limits<float>::max())
349 continue;
350
351 (*maxima)(j, i).intensity = std::numeric_limits<float>::max();
352 cannyTraceEdge(1, 0, i, j, *maxima);
353 cannyTraceEdge(-1, 0, i, j, *maxima);
354 cannyTraceEdge(1, 1, i, j, *maxima);
355 cannyTraceEdge(-1, -1, i, j, *maxima);
356 cannyTraceEdge(0, -1, i, j, *maxima);
357 cannyTraceEdge(0, 1, i, j, *maxima);
358 cannyTraceEdge(-1, 1, i, j, *maxima);
359 cannyTraceEdge(1, -1, i, j, *maxima);
360 }
361 }
362
363 // Final thresholding
364 for (std::size_t i = 0; i < input_->size(); ++i) {
365 if ((*maxima)[i].intensity == std::numeric_limits<float>::max())
366 output[i].magnitude = 255;
367 else
368 output[i].magnitude = 0;
369 }
370}
371
372template <typename PointInT, typename PointOutT>
373void
375 const pcl::PointCloud<PointInT>& input_y,
377{
378 float tHigh = hysteresis_threshold_high_;
379 float tLow = hysteresis_threshold_low_;
380 const int height = input_x.height;
381 const int width = input_x.width;
382
383 output.resize(height * width);
384 output.height = height;
385 output.width = width;
386
387 // Noise reduction using gaussian blurring
389 kernel_.setKernelSize(3);
390 kernel_.setKernelSigma(1.0);
391 kernel_.setKernelType(kernel<PointXYZI>::GAUSSIAN);
392 kernel_.fetchKernel(*gaussian_kernel);
393 convolution_.setKernel(*gaussian_kernel);
394
395 PointCloudIn smoothed_cloud_x;
396 convolution_.setInputCloud(input_x.makeShared());
397 convolution_.filter(smoothed_cloud_x);
398
399 PointCloudIn smoothed_cloud_y;
400 convolution_.setInputCloud(input_y.makeShared());
401 convolution_.filter(smoothed_cloud_y);
402
403 // Edge detection using Sobel
405 sobelMagnitudeDirection(smoothed_cloud_x, smoothed_cloud_y, *edges.get());
406
407 // Edge discretization
408 discretizeAngles(*edges);
409
411 suppressNonMaxima(*edges, *maxima, tLow);
412
413 // Edge tracing
414 for (int i = 0; i < height; i++) {
415 for (int j = 0; j < width; j++) {
416 if ((*maxima)(j, i).intensity < tHigh ||
417 (*maxima)(j, i).intensity == std::numeric_limits<float>::max())
418 continue;
419
420 (*maxima)(j, i).intensity = std::numeric_limits<float>::max();
421
422 // clang-format off
423 cannyTraceEdge( 1, 0, i, j, *maxima);
424 cannyTraceEdge(-1, 0, i, j, *maxima);
425 cannyTraceEdge( 1, 1, i, j, *maxima);
426 cannyTraceEdge(-1, -1, i, j, *maxima);
427 cannyTraceEdge( 0, -1, i, j, *maxima);
428 cannyTraceEdge( 0, 1, i, j, *maxima);
429 cannyTraceEdge(-1, 1, i, j, *maxima);
430 cannyTraceEdge( 1, -1, i, j, *maxima);
431 // clang-format on
432 }
433 }
434
435 // Final thresholding
436 for (int i = 0; i < height; i++) {
437 for (int j = 0; j < width; j++) {
438 if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max())
439 output(j, i).magnitude = 255;
440 else
441 output(j, i).magnitude = 0;
442 }
443 }
444}
445
446template <typename PointInT, typename PointOutT>
447void
449 const float kernel_size,
451{
452 convolution_.setInputCloud(input_);
453
455 kernel_.setKernelType(kernel<PointXYZI>::LOG);
456 kernel_.setKernelSigma(kernel_sigma);
457 kernel_.setKernelSize(kernel_size);
458 kernel_.fetchKernel(*log_kernel);
459 convolution_.setKernel(*log_kernel);
460 convolution_.filter(output);
461}
462
463} // namespace pcl
Define standard C methods to do angle calculations.
void detectEdgePrewitt(pcl::PointCloud< PointOutT > &output)
Uses the Prewitt kernel for edge detection.
Definition edge.hpp:126
void detectEdgeLoG(const float kernel_sigma, const float kernel_size, pcl::PointCloud< PointOutT > &output)
Uses the LoG kernel for edge detection.
Definition edge.hpp:448
void detectEdgeSobel(pcl::PointCloud< PointOutT > &output)
Uses the Sobel kernel for edge detection.
Definition edge.hpp:48
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:374
void sobelMagnitudeDirection(const pcl::PointCloud< PointInT > &input_x, const pcl::PointCloud< PointInT > &input_y, pcl::PointCloud< PointOutT > &output)
Definition edge.hpp:85
void detectEdgeRoberts(pcl::PointCloud< PointOutT > &output)
Uses the Roberts kernel for edge detection.
Definition edge.hpp:164
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.
Definition edge.hpp:310
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t size() const
shared_ptr< PointCloud< PointT > > Ptr
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed,...
float rad2deg(float alpha)
Convert an angle from radians to degrees.
Definition angles.hpp:61