38 #ifndef PCL_2D_EDGE_IMPL_HPP
39 #define PCL_2D_EDGE_IMPL_HPP
41 #include <pcl/2d/convolution.h>
42 #include <pcl/common/common_headers.h>
45 template <
typename Po
intInT,
typename Po
intOutT>
49 convolution_.setInputCloud(input_);
53 kernel_.fetchKernel(*kernel_x);
54 convolution_.setKernel(*kernel_x);
55 convolution_.filter(*magnitude_x);
60 kernel_.fetchKernel(*kernel_y);
61 convolution_.setKernel(*kernel_y);
62 convolution_.filter(*magnitude_y);
64 const int height = input_->height;
65 const int width = input_->width;
67 output.
resize(height * width);
71 for (std::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;
75 std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
76 (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
78 std::atan2((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
83 template <
typename Po
intInT,
typename Po
intOutT>
90 convolution_.setInputCloud(input_x.
makeShared());
94 kernel_.fetchKernel(*kernel_x);
95 convolution_.setKernel(*kernel_x);
96 convolution_.filter(*magnitude_x);
98 convolution_.setInputCloud(input_y.
makeShared());
102 kernel_.fetchKernel(*kernel_y);
103 convolution_.setKernel(*kernel_y);
104 convolution_.filter(*magnitude_y);
106 const int height = input_x.
height;
107 const int width = input_x.
width;
109 output.
resize(height * width);
111 output.
width = width;
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);
125 template <
typename Po
intInT,
typename Po
intOutT>
129 convolution_.setInputCloud(input_);
134 kernel_.fetchKernel(*kernel_x);
135 convolution_.setKernel(*kernel_x);
136 convolution_.filter(*magnitude_x);
141 kernel_.fetchKernel(*kernel_y);
142 convolution_.setKernel(*kernel_y);
143 convolution_.filter(*magnitude_y);
145 const int height = input_->height;
146 const int width = input_->width;
148 output.
resize(height * width);
150 output.
width = width;
152 for (std::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);
164 template <
typename Po
intInT,
typename Po
intOutT>
168 convolution_.setInputCloud(input_);
173 kernel_.fetchKernel(*kernel_x);
174 convolution_.setKernel(*kernel_x);
175 convolution_.filter(*magnitude_x);
180 kernel_.fetchKernel(*kernel_y);
181 convolution_.setKernel(*kernel_y);
182 convolution_.filter(*magnitude_y);
184 const int height = input_->height;
185 const int width = input_->width;
187 output.
resize(height * width);
189 output.
width = width;
191 for (std::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);
203 template <
typename Po
intInT,
typename Po
intOutT>
208 int newRow = row + rowOffset;
209 int newCol = col + colOffset;
212 if (newRow > 0 && newRow <
static_cast<int>(maxima.
height) && newCol > 0 &&
213 newCol <
static_cast<int>(maxima.
width)) {
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);
230 template <
typename Po
intInT,
typename Po
intOutT>
234 const int height = thet.
height;
235 const int width = thet.
width;
237 for (
int i = 0; i < height; i++) {
238 for (
int j = 0; j < width; j++) {
240 if (((angle <= 22.5) && (angle >= -22.5)) || (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;
257 template <
typename Po
intInT,
typename Po
intOutT>
264 const int height = edges.
height;
265 const int width = edges.
width;
268 maxima.
width = width;
269 maxima.
resize(height * width);
271 for (
auto& point : maxima)
272 point.intensity = 0.0f;
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);
280 if (ptedge.magnitude < tLow)
285 switch (
int(ptedge.direction)) {
287 if (ptedge.magnitude >= edges(j - 1, i).magnitude &&
288 ptedge.magnitude >= edges(j + 1, i).magnitude)
289 ptmax.intensity = ptedge.magnitude;
293 if (ptedge.magnitude >= edges(j - 1, i - 1).magnitude &&
294 ptedge.magnitude >= edges(j + 1, i + 1).magnitude)
295 ptmax.intensity = ptedge.magnitude;
299 if (ptedge.magnitude >= edges(j, i - 1).magnitude &&
300 ptedge.magnitude >= edges(j, i + 1).magnitude)
301 ptmax.intensity = ptedge.magnitude;
305 if (ptedge.magnitude >= edges(j + 1, i - 1).magnitude &&
306 ptedge.magnitude >= edges(j - 1, i + 1).magnitude)
307 ptmax.intensity = ptedge.magnitude;
316 template <
typename Po
intInT,
typename Po
intOutT>
320 float tHigh = hysteresis_threshold_high_;
321 float tLow = hysteresis_threshold_low_;
322 const int height = input_->height;
323 const int width = input_->width;
325 output.
resize(height * width);
327 output.
width = width;
332 kernel_.setKernelSize(3);
333 kernel_.setKernelSigma(1.0);
335 kernel_.fetchKernel(*gaussian_kernel);
336 convolution_.setKernel(*gaussian_kernel);
337 convolution_.setInputCloud(input_);
338 convolution_.filter(*smoothed_cloud);
342 setInputCloud(smoothed_cloud);
343 detectEdgeSobel(*edges);
346 discretizeAngles(*edges);
350 suppressNonMaxima(*edges, *maxima, tLow);
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())
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);
372 for (std::size_t i = 0; i < input_->size(); ++i) {
373 if ((*maxima)[i].intensity == std::numeric_limits<float>::max())
374 output[i].magnitude = 255;
376 output[i].magnitude = 0;
381 template <
typename Po
intInT,
typename Po
intOutT>
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;
392 output.
resize(height * width);
394 output.
width = width;
398 kernel_.setKernelSize(3);
399 kernel_.setKernelSigma(1.0);
401 kernel_.fetchKernel(*gaussian_kernel);
402 convolution_.setKernel(*gaussian_kernel);
405 convolution_.setInputCloud(input_x.
makeShared());
406 convolution_.filter(smoothed_cloud_x);
409 convolution_.setInputCloud(input_y.
makeShared());
410 convolution_.filter(smoothed_cloud_y);
414 sobelMagnitudeDirection(smoothed_cloud_x, smoothed_cloud_y, *edges.get());
417 discretizeAngles(*edges);
420 suppressNonMaxima(*edges, *maxima, tLow);
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())
429 (*maxima)(j, i).intensity = std::numeric_limits<float>::max();
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);
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;
450 output(j, i).magnitude = 0;
456 template <
typename Po
intInT,
typename Po
intOutT>
459 const float kernel_size,
462 convolution_.setInputCloud(input_);
466 kernel_.setKernelSigma(kernel_sigma);
467 kernel_.setKernelSize(kernel_size);
468 kernel_.fetchKernel(*log_kernel);
469 convolution_.setKernel(*log_kernel);
470 convolution_.filter(output);