40 #ifndef PCL_FILTERS_CONVOLUTION_IMPL_HPP
41 #define PCL_FILTERS_CONVOLUTION_IMPL_HPP
43 #include <pcl/pcl_config.h>
46 template <
typename Po
intIn,
typename Po
intOut>
48 : borders_policy_ (BORDERS_POLICY_IGNORE)
49 , distance_threshold_ (std::numeric_limits<float>::infinity ())
56 template <
typename Po
intIn,
typename Po
intOut>
void
59 if (borders_policy_ != BORDERS_POLICY_IGNORE &&
60 borders_policy_ != BORDERS_POLICY_MIRROR &&
61 borders_policy_ != BORDERS_POLICY_DUPLICATE)
63 "[pcl::filters::Convolution::initCompute] unknown borders policy.");
65 if(kernel_.size () % 2 == 0)
67 "[pcl::filters::Convolution::initCompute] convolving element width must be odd.");
69 if (distance_threshold_ != std::numeric_limits<float>::infinity ())
70 distance_threshold_ *=
static_cast<float> (kernel_.size () % 2) * distance_threshold_;
72 half_width_ =
static_cast<int> (kernel_.size ()) / 2;
73 kernel_width_ =
static_cast<int> (kernel_.size () - 1);
75 if (&(*input_) != &output)
77 if (output.
height != input_->height || output.
width != input_->width)
79 output.
resize (input_->width * input_->height);
80 output.
width = input_->width;
81 output.
height = input_->height;
87 template <
typename Po
intIn,
typename Po
intOut>
inline void
93 switch (borders_policy_)
95 case BORDERS_POLICY_MIRROR : convolve_rows_mirror (output);
break;
96 case BORDERS_POLICY_DUPLICATE : convolve_rows_duplicate (output);
break;
97 case BORDERS_POLICY_IGNORE : convolve_rows (output);
103 "[pcl::filters::Convolution::convolveRows] init failed " << e.what ());
107 template <
typename Po
intIn,
typename Po
intOut>
inline void
112 initCompute (output);
113 switch (borders_policy_)
115 case BORDERS_POLICY_MIRROR : convolve_cols_mirror (output);
break;
116 case BORDERS_POLICY_DUPLICATE : convolve_cols_duplicate (output);
break;
117 case BORDERS_POLICY_IGNORE : convolve_cols (output);
123 "[pcl::filters::Convolution::convolveCols] init failed " << e.what ());
127 template <
typename Po
intIn,
typename Po
intOut>
inline void
129 const Eigen::ArrayXf& v_kernel,
135 setKernel (h_kernel);
138 setKernel (v_kernel);
139 convolveCols (output);
144 "[pcl::filters::Convolution::convolve] init failed " << e.what ());
148 template <
typename Po
intIn,
typename Po
intOut>
inline void
156 convolveCols (output);
161 "[pcl::filters::Convolution::convolve] init failed " << e.what ());
165 template <
typename Po
intIn,
typename Po
intOut>
inline PointOut
170 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
171 result+= (*input_) (l,j) * kernel_[k];
175 template <
typename Po
intIn,
typename Po
intOut>
inline PointOut
180 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
181 result+= (*input_) (i,l) * kernel_[k];
185 template <
typename Po
intIn,
typename Po
intOut>
inline PointOut
191 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
197 result+= (*input_) (l,j) * kernel_[k];
198 weight += kernel_[k];
202 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
211 template <
typename Po
intIn,
typename Po
intOut>
inline PointOut
217 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
223 result+= (*input_) (i,l) * kernel_[k];
224 weight += kernel_[k];
228 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
242 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowDense (
int i,
int j)
245 float r = 0, g = 0, b = 0;
246 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
248 result.x += (*input_) (l,j).x * kernel_[k];
249 result.y += (*input_) (l,j).y * kernel_[k];
250 result.z += (*input_) (l,j).z * kernel_[k];
251 r += kernel_[k] *
static_cast<float> ((*input_) (l,j).r);
252 g += kernel_[k] *
static_cast<float> ((*input_) (l,j).g);
253 b += kernel_[k] *
static_cast<float> ((*input_) (l,j).b);
255 result.r =
static_cast<std::uint8_t
> (r);
256 result.g =
static_cast<std::uint8_t
> (g);
257 result.b =
static_cast<std::uint8_t
> (b);
262 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColDense (
int i,
int j)
265 float r = 0, g = 0, b = 0;
266 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
268 result.x += (*input_) (i,l).x * kernel_[k];
269 result.y += (*input_) (i,l).y * kernel_[k];
270 result.z += (*input_) (i,l).z * kernel_[k];
271 r += kernel_[k] *
static_cast<float> ((*input_) (i,l).r);
272 g += kernel_[k] *
static_cast<float> ((*input_) (i,l).g);
273 b += kernel_[k] *
static_cast<float> ((*input_) (i,l).b);
275 result.r =
static_cast<std::uint8_t
> (r);
276 result.g =
static_cast<std::uint8_t
> (g);
277 result.b =
static_cast<std::uint8_t
> (b);
282 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowNonDense (
int i,
int j)
286 float r = 0, g = 0, b = 0;
287 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
293 result.x += (*input_) (l,j).x * kernel_[k]; result.y += (*input_) (l,j).y * kernel_[k]; result.z += (*input_) (l,j).z * kernel_[k];
294 r+= kernel_[k] *
static_cast<float> ((*input_) (l,j).r);
295 g+= kernel_[k] *
static_cast<float> ((*input_) (l,j).g);
296 b+= kernel_[k] *
static_cast<float> ((*input_) (l,j).b);
297 weight += kernel_[k];
302 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
306 r*= weight; g*= weight; b*= weight;
307 result.x*= weight; result.y*= weight; result.z*= weight;
308 result.r =
static_cast<std::uint8_t
> (r);
309 result.g =
static_cast<std::uint8_t
> (g);
310 result.b =
static_cast<std::uint8_t
> (b);
316 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColNonDense (
int i,
int j)
320 float r = 0, g = 0, b = 0;
321 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
327 result.x += (*input_) (i,l).x * kernel_[k]; result.y += (*input_) (i,l).y * kernel_[k]; result.z += (*input_) (i,l).z * kernel_[k];
328 r+= kernel_[k] *
static_cast<float> ((*input_) (i,l).r);
329 g+= kernel_[k] *
static_cast<float> ((*input_) (i,l).g);
330 b+= kernel_[k] *
static_cast<float> ((*input_) (i,l).b);
335 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
339 r*= weight; g*= weight; b*= weight;
340 result.x*= weight; result.y*= weight; result.z*= weight;
341 result.r =
static_cast<std::uint8_t
> (r);
342 result.g =
static_cast<std::uint8_t
> (g);
343 result.b =
static_cast<std::uint8_t
> (b);
350 Convolution<pcl::RGB, pcl::RGB>::convolveOneRowDense (
int i,
int j)
353 float r = 0, g = 0, b = 0;
354 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
356 r += kernel_[k] *
static_cast<float> ((*input_) (l,j).r);
357 g += kernel_[k] *
static_cast<float> ((*input_) (l,j).g);
358 b += kernel_[k] *
static_cast<float> ((*input_) (l,j).b);
360 result.r =
static_cast<std::uint8_t
> (r);
361 result.g =
static_cast<std::uint8_t
> (g);
362 result.b =
static_cast<std::uint8_t
> (b);
367 Convolution<pcl::RGB, pcl::RGB>::convolveOneColDense (
int i,
int j)
370 float r = 0, g = 0, b = 0;
371 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
373 r += kernel_[k] *
static_cast<float> ((*input_) (i,l).r);
374 g += kernel_[k] *
static_cast<float> ((*input_) (i,l).g);
375 b += kernel_[k] *
static_cast<float> ((*input_) (i,l).b);
377 result.r =
static_cast<std::uint8_t
> (r);
378 result.g =
static_cast<std::uint8_t
> (g);
379 result.b =
static_cast<std::uint8_t
> (b);
384 Convolution<pcl::RGB, pcl::RGB>::convolveOneRowNonDense (
int i,
int j)
386 return (convolveOneRowDense (i,j));
390 Convolution<pcl::RGB, pcl::RGB>::convolveOneColNonDense (
int i,
int j)
392 return (convolveOneColDense (i,j));
398 p.r = 0; p.g = 0; p.b = 0;
403 template <
typename Po
intIn,
typename Po
intOut>
void
408 int width = input_->width;
409 int height = input_->height;
410 int last = input_->width - half_width_;
411 if (input_->is_dense)
414 #pragma omp parallel for shared (output) num_threads (threads_)
416 for(
int j = 0; j < height; ++j)
418 for (
int i = 0; i < half_width_; ++i)
419 makeInfinite (output (i,j));
421 for (
int i = half_width_; i < last; ++i)
422 output (i,j) = convolveOneRowDense (i,j);
424 for (
int i = last; i < width; ++i)
425 makeInfinite (output (i,j));
431 #pragma omp parallel for shared (output) num_threads (threads_)
433 for(
int j = 0; j < height; ++j)
435 for (
int i = 0; i < half_width_; ++i)
436 makeInfinite (output (i,j));
438 for (
int i = half_width_; i < last; ++i)
439 output (i,j) = convolveOneRowNonDense (i,j);
441 for (
int i = last; i < width; ++i)
442 makeInfinite (output (i,j));
447 template <
typename Po
intIn,
typename Po
intOut>
void
452 int width = input_->width;
453 int height = input_->height;
454 int last = input_->width - half_width_;
456 if (input_->is_dense)
459 #pragma omp parallel for shared (output) num_threads (threads_)
461 for(
int j = 0; j < height; ++j)
463 for (
int i = half_width_; i < last; ++i)
464 output (i,j) = convolveOneRowDense (i,j);
466 for (
int i = last; i < width; ++i)
467 output (i,j) = output (w, j);
469 for (
int i = 0; i < half_width_; ++i)
470 output (i,j) = output (half_width_, j);
476 #pragma omp parallel for shared (output) num_threads (threads_)
478 for(
int j = 0; j < height; ++j)
480 for (
int i = half_width_; i < last; ++i)
481 output (i,j) = convolveOneRowNonDense (i,j);
483 for (
int i = last; i < width; ++i)
484 output (i,j) = output (w, j);
486 for (
int i = 0; i < half_width_; ++i)
487 output (i,j) = output (half_width_, j);
492 template <
typename Po
intIn,
typename Po
intOut>
void
497 int width = input_->width;
498 int height = input_->height;
499 int last = input_->width - half_width_;
501 if (input_->is_dense)
504 #pragma omp parallel for shared (output) num_threads (threads_)
506 for(
int j = 0; j < height; ++j)
508 for (
int i = half_width_; i < last; ++i)
509 output (i,j) = convolveOneRowDense (i,j);
511 for (
int i = last, l = 0; i < width; ++i, ++l)
512 output (i,j) = output (w-l, j);
514 for (
int i = 0; i < half_width_; ++i)
515 output (i,j) = output (half_width_+1-i, j);
521 #pragma omp parallel for shared (output) num_threads (threads_)
523 for(
int j = 0; j < height; ++j)
525 for (
int i = half_width_; i < last; ++i)
526 output (i,j) = convolveOneRowNonDense (i,j);
528 for (
int i = last, l = 0; i < width; ++i, ++l)
529 output (i,j) = output (w-l, j);
531 for (
int i = 0; i < half_width_; ++i)
532 output (i,j) = output (half_width_+1-i, j);
537 template <
typename Po
intIn,
typename Po
intOut>
void
542 int width = input_->width;
543 int height = input_->height;
544 int last = input_->height - half_width_;
545 if (input_->is_dense)
548 #pragma omp parallel for shared (output) num_threads (threads_)
550 for(
int i = 0; i < width; ++i)
552 for (
int j = 0; j < half_width_; ++j)
553 makeInfinite (output (i,j));
555 for (
int j = half_width_; j < last; ++j)
556 output (i,j) = convolveOneColDense (i,j);
558 for (
int j = last; j < height; ++j)
559 makeInfinite (output (i,j));
565 #pragma omp parallel for shared (output) num_threads (threads_)
567 for(
int i = 0; i < width; ++i)
569 for (
int j = 0; j < half_width_; ++j)
570 makeInfinite (output (i,j));
572 for (
int j = half_width_; j < last; ++j)
573 output (i,j) = convolveOneColNonDense (i,j);
575 for (
int j = last; j < height; ++j)
576 makeInfinite (output (i,j));
581 template <
typename Po
intIn,
typename Po
intOut>
void
586 int width = input_->width;
587 int height = input_->height;
588 int last = input_->height - half_width_;
590 if (input_->is_dense)
593 #pragma omp parallel for shared (output) num_threads (threads_)
595 for(
int i = 0; i < width; ++i)
597 for (
int j = half_width_; j < last; ++j)
598 output (i,j) = convolveOneColDense (i,j);
600 for (
int j = last; j < height; ++j)
601 output (i,j) = output (i,h);
603 for (
int j = 0; j < half_width_; ++j)
604 output (i,j) = output (i, half_width_);
610 #pragma omp parallel for shared (output) num_threads (threads_)
612 for(
int i = 0; i < width; ++i)
614 for (
int j = half_width_; j < last; ++j)
615 output (i,j) = convolveOneColNonDense (i,j);
617 for (
int j = last; j < height; ++j)
618 output (i,j) = output (i,h);
620 for (
int j = 0; j < half_width_; ++j)
621 output (i,j) = output (i,half_width_);
626 template <
typename Po
intIn,
typename Po
intOut>
void
631 int width = input_->width;
632 int height = input_->height;
633 int last = input_->height - half_width_;
635 if (input_->is_dense)
638 #pragma omp parallel for shared (output) num_threads (threads_)
640 for(
int i = 0; i < width; ++i)
642 for (
int j = half_width_; j < last; ++j)
643 output (i,j) = convolveOneColDense (i,j);
645 for (
int j = last, l = 0; j < height; ++j, ++l)
646 output (i,j) = output (i,h-l);
648 for (
int j = 0; j < half_width_; ++j)
649 output (i,j) = output (i, half_width_+1-j);
655 #pragma omp parallel for shared (output) num_threads (threads_)
657 for(
int i = 0; i < width; ++i)
659 for (
int j = half_width_; j < last; ++j)
660 output (i,j) = convolveOneColNonDense (i,j);
662 for (
int j = last, l = 0; j < height; ++j, ++l)
663 output (i,j) = output (i,h-l);
665 for (
int j = 0; j < half_width_; ++j)
666 output (i,j) = output (i,half_width_+1-j);
671 #endif //PCL_FILTERS_CONVOLUTION_IMPL_HPP