40 #include <pcl/2d/convolution.h>
41 #include <pcl/2d/edge.h>
46 template <
typename Po
intInT,
typename Po
intOutT>
50 convolution_.setInputCloud(input_);
54 kernel_.fetchKernel(*kernel_x);
55 convolution_.setKernel(*kernel_x);
56 convolution_.filter(*magnitude_x);
61 kernel_.fetchKernel(*kernel_y);
62 convolution_.setKernel(*kernel_y);
63 convolution_.filter(*magnitude_y);
65 const int height = input_->height;
66 const int width = input_->width;
68 output.
resize(height * width);
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;
76 std::sqrt((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
77 (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
79 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);
124 template <
typename Po
intInT,
typename Po
intOutT>
128 convolution_.setInputCloud(input_);
133 kernel_.fetchKernel(*kernel_x);
134 convolution_.setKernel(*kernel_x);
135 convolution_.filter(*magnitude_x);
140 kernel_.fetchKernel(*kernel_y);
141 convolution_.setKernel(*kernel_y);
142 convolution_.filter(*magnitude_y);
144 const int height = input_->height;
145 const int width = input_->width;
147 output.
resize(height * width);
149 output.
width = width;
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);
162 template <
typename Po
intInT,
typename Po
intOutT>
166 convolution_.setInputCloud(input_);
171 kernel_.fetchKernel(*kernel_x);
172 convolution_.setKernel(*kernel_x);
173 convolution_.filter(*magnitude_x);
178 kernel_.fetchKernel(*kernel_y);
179 convolution_.setKernel(*kernel_y);
180 convolution_.filter(*magnitude_y);
182 const int height = input_->height;
183 const int width = input_->width;
187 output.
width = width;
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);
200 template <
typename Po
intInT,
typename Po
intOutT>
202 Edge<PointInT, PointOutT>::cannyTraceEdge(
205 int newRow = row + rowOffset;
206 int newCol = col + colOffset;
209 if (newRow > 0 && newRow <
static_cast<int>(maxima.
height) && newCol > 0 &&
210 newCol <
static_cast<int>(maxima.
width)) {
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);
226 template <
typename Po
intInT,
typename Po
intOutT>
230 const int height = thet.
height;
231 const int width = thet.
width;
233 for (
int i = 0; i < height; i++) {
234 for (
int j = 0; j < width; j++) {
236 if (((angle <= 22.5) && (angle >= -22.5)) || (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;
252 template <
typename Po
intInT,
typename Po
intOutT>
254 Edge<PointInT, PointOutT>::suppressNonMaxima(
259 const int height = edges.
height;
260 const int width = edges.
width;
263 maxima.
width = width;
264 maxima.
resize(height * width);
266 for (
auto& point : maxima)
267 point.intensity = 0.0f;
270 for (
int i = 1; i < height - 1; i++) {
271 for (
int j = 1; j < width - 1; j++) {
272 const PointXYZIEdge& ptedge = edges(j, i);
273 PointXYZI& ptmax = maxima(j, i);
275 if (ptedge.magnitude < tLow)
280 switch (
static_cast<int>(ptedge.direction)) {
282 if (ptedge.magnitude >= edges(j - 1, i).magnitude &&
283 ptedge.magnitude >= edges(j + 1, i).magnitude)
284 ptmax.intensity = ptedge.magnitude;
288 if (ptedge.magnitude >= edges(j - 1, i - 1).magnitude &&
289 ptedge.magnitude >= edges(j + 1, i + 1).magnitude)
290 ptmax.intensity = ptedge.magnitude;
294 if (ptedge.magnitude >= edges(j, i - 1).magnitude &&
295 ptedge.magnitude >= edges(j, i + 1).magnitude)
296 ptmax.intensity = ptedge.magnitude;
300 if (ptedge.magnitude >= edges(j + 1, i - 1).magnitude &&
301 ptedge.magnitude >= edges(j - 1, i + 1).magnitude)
302 ptmax.intensity = ptedge.magnitude;
310 template <
typename Po
intInT,
typename Po
intOutT>
314 float tHigh = hysteresis_threshold_high_;
315 float tLow = hysteresis_threshold_low_;
316 const int height = input_->height;
317 const int width = input_->width;
319 output.
resize(height * width);
321 output.
width = width;
326 kernel_.setKernelSize(3);
327 kernel_.setKernelSigma(1.0);
329 kernel_.fetchKernel(*gaussian_kernel);
330 convolution_.setKernel(*gaussian_kernel);
331 convolution_.setInputCloud(input_);
332 convolution_.filter(*smoothed_cloud);
336 setInputCloud(smoothed_cloud);
337 detectEdgeSobel(*edges);
340 discretizeAngles(*edges);
344 suppressNonMaxima(*edges, *maxima, tLow);
347 for (
int i = 0; i < height; i++) {
348 for (
int j = 0; j < width; j++) {
349 if ((*maxima)(j, i).intensity < tHigh ||
350 (*maxima)(j, i).intensity == std::numeric_limits<float>::max())
353 (*maxima)(j, i).intensity = std::numeric_limits<float>::max();
354 cannyTraceEdge(1, 0, i, j, *maxima);
355 cannyTraceEdge(-1, 0, i, j, *maxima);
356 cannyTraceEdge(1, 1, i, j, *maxima);
357 cannyTraceEdge(-1, -1, i, j, *maxima);
358 cannyTraceEdge(0, -1, i, j, *maxima);
359 cannyTraceEdge(0, 1, i, j, *maxima);
360 cannyTraceEdge(-1, 1, i, j, *maxima);
361 cannyTraceEdge(1, -1, i, j, *maxima);
366 for (std::size_t i = 0; i < input_->size(); ++i) {
367 if ((*maxima)[i].intensity == std::numeric_limits<float>::max())
368 output[i].magnitude = 255;
370 output[i].magnitude = 0;
374 template <
typename Po
intInT,
typename Po
intOutT>
380 float tHigh = hysteresis_threshold_high_;
381 float tLow = hysteresis_threshold_low_;
382 const int height = input_x.
height;
383 const int width = input_x.
width;
385 output.
resize(height * width);
387 output.
width = width;
391 kernel_.setKernelSize(3);
392 kernel_.setKernelSigma(1.0);
394 kernel_.fetchKernel(*gaussian_kernel);
395 convolution_.setKernel(*gaussian_kernel);
398 convolution_.setInputCloud(input_x.
makeShared());
399 convolution_.filter(smoothed_cloud_x);
402 convolution_.setInputCloud(input_y.
makeShared());
403 convolution_.filter(smoothed_cloud_y);
407 sobelMagnitudeDirection(smoothed_cloud_x, smoothed_cloud_y, *edges.get());
410 discretizeAngles(*edges);
413 suppressNonMaxima(*edges, *maxima, tLow);
416 for (
int i = 0; i < height; i++) {
417 for (
int j = 0; j < width; j++) {
418 if ((*maxima)(j, i).intensity < tHigh ||
419 (*maxima)(j, i).intensity == std::numeric_limits<float>::max())
422 (*maxima)(j, i).intensity = std::numeric_limits<float>::max();
425 cannyTraceEdge( 1, 0, i, j, *maxima);
426 cannyTraceEdge(-1, 0, i, j, *maxima);
427 cannyTraceEdge( 1, 1, i, j, *maxima);
428 cannyTraceEdge(-1, -1, i, j, *maxima);
429 cannyTraceEdge( 0, -1, i, j, *maxima);
430 cannyTraceEdge( 0, 1, i, j, *maxima);
431 cannyTraceEdge(-1, 1, i, j, *maxima);
432 cannyTraceEdge( 1, -1, i, j, *maxima);
438 for (
int i = 0; i < height; i++) {
439 for (
int j = 0; j < width; j++) {
440 if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max())
441 output(j, i).magnitude = 255;
443 output(j, i).magnitude = 0;
448 template <
typename Po
intInT,
typename Po
intOutT>
451 const float kernel_size,
454 convolution_.setInputCloud(input_);
458 kernel_.setKernelSigma(kernel_sigma);
459 kernel_.setKernelSize(kernel_size);
460 kernel_.fetchKernel(*log_kernel);
461 convolution_.setKernel(*log_kernel);
462 convolution_.filter(output);
Define standard C methods to do angle calculations.
void detectEdgePrewitt(pcl::PointCloud< PointOutT > &output)
Uses the Prewitt kernel for edge detection.
void detectEdgeLoG(const float kernel_sigma, const float kernel_size, pcl::PointCloud< PointOutT > &output)
Uses the LoG kernel for edge detection.
void detectEdgeSobel(pcl::PointCloud< PointOutT > &output)
Uses the Sobel kernel for edge detection.
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.
void sobelMagnitudeDirection(const pcl::PointCloud< PointInT > &input_x, const pcl::PointCloud< PointInT > &input_y, pcl::PointCloud< PointOutT > &output)
void detectEdgeRoberts(pcl::PointCloud< PointOutT > &output)
Uses the Roberts kernel for edge detection.
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.
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).
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.