40 #ifndef PCL_FILTERS_CONVOLUTION_IMPL_HPP
41 #define PCL_FILTERS_CONVOLUTION_IMPL_HPP
43 #include <pcl/pcl_config.h>
44 #include <pcl/common/distances.h>
46 template <
typename Po
intIn,
typename Po
intOut>
48 : borders_policy_ (BORDERS_POLICY_IGNORE)
49 , distance_threshold_ (std::numeric_limits<float>::infinity ())
57 template <
typename Po
intIn,
typename Po
intOut>
void
60 if (borders_policy_ != BORDERS_POLICY_IGNORE &&
61 borders_policy_ != BORDERS_POLICY_MIRROR &&
62 borders_policy_ != BORDERS_POLICY_DUPLICATE)
64 "[pcl::filters::Convolution::initCompute] unknown borders policy.");
66 if(kernel_.size () % 2 == 0)
68 "[pcl::filters::Convolution::initCompute] convolving element width must be odd.");
70 if (distance_threshold_ != std::numeric_limits<float>::infinity ())
71 distance_threshold_ *= static_cast<float> (kernel_.size () % 2) * distance_threshold_;
73 half_width_ =
static_cast<int> (kernel_.size ()) / 2;
74 kernel_width_ =
static_cast<int> (kernel_.size () - 1);
76 if (&(*input_) != &output)
78 if (output.
height != input_->height || output.
width != input_->width)
80 output.
resize (input_->width * input_->height);
81 output.
width = input_->width;
82 output.
height = input_->height;
88 template <
typename Po
intIn,
typename Po
intOut>
inline void
94 switch (borders_policy_)
96 case BORDERS_POLICY_MIRROR : convolve_rows_mirror (output);
97 case BORDERS_POLICY_DUPLICATE : convolve_rows_duplicate (output);
98 case BORDERS_POLICY_IGNORE : convolve_rows (output);
104 "[pcl::filters::Convolution::convolveRows] init failed " << e.
what ());
108 template <
typename Po
intIn,
typename Po
intOut>
inline void
113 initCompute (output);
114 switch (borders_policy_)
116 case BORDERS_POLICY_MIRROR : convolve_cols_mirror (output);
117 case BORDERS_POLICY_DUPLICATE : convolve_cols_duplicate (output);
118 case BORDERS_POLICY_IGNORE : convolve_cols (output);
124 "[pcl::filters::Convolution::convolveCols] init failed " << e.
what ());
128 template <
typename Po
intIn,
typename Po
intOut>
inline void
130 const Eigen::ArrayXf& v_kernel,
136 setKernel (h_kernel);
139 setKernel (v_kernel);
140 convolveCols (output);
145 "[pcl::filters::Convolution::convolve] init failed " << e.
what ());
149 template <
typename Po
intIn,
typename Po
intOut>
inline void
157 convolveCols (output);
162 "[pcl::filters::Convolution::convolve] init failed " << e.
what ());
166 template <
typename Po
intIn,
typename Po
intOut>
inline PointOut
169 using namespace pcl::common;
171 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
172 result+= (*input_) (l,j) * kernel_[k];
176 template <
typename Po
intIn,
typename Po
intOut>
inline PointOut
179 using namespace pcl::common;
181 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
182 result+= (*input_) (i,l) * kernel_[k];
186 template <
typename Po
intIn,
typename Po
intOut>
inline PointOut
189 using namespace pcl::common;
192 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
198 result+= (*input_) (l,j) * kernel_[k];
199 weight += kernel_[k];
203 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
212 template <
typename Po
intIn,
typename Po
intOut>
inline PointOut
215 using namespace pcl::common;
218 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
224 result+= (*input_) (i,l) * kernel_[k];
225 weight += kernel_[k];
229 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
243 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowDense (
int i,
int j)
246 float r = 0, g = 0, b = 0;
247 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
249 result.x += (*input_) (l,j).x * kernel_[k];
250 result.y += (*input_) (l,j).y * kernel_[k];
251 result.z += (*input_) (l,j).z * kernel_[k];
252 r += kernel_[k] *
static_cast<float> ((*input_) (l,j).r);
253 g += kernel_[k] *
static_cast<float> ((*input_) (l,j).g);
254 b += kernel_[k] *
static_cast<float> ((*input_) (l,j).b);
256 result.r =
static_cast<pcl::uint8_t
> (r);
257 result.g =
static_cast<pcl::uint8_t
> (g);
258 result.b =
static_cast<pcl::uint8_t
> (b);
263 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColDense (
int i,
int j)
266 float r = 0, g = 0, b = 0;
267 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
269 result.x += (*input_) (i,l).x * kernel_[k];
270 result.y += (*input_) (i,l).y * kernel_[k];
271 result.z += (*input_) (i,l).z * kernel_[k];
272 r += kernel_[k] *
static_cast<float> ((*input_) (i,l).r);
273 g += kernel_[k] *
static_cast<float> ((*input_) (i,l).g);
274 b += kernel_[k] *
static_cast<float> ((*input_) (i,l).b);
276 result.r =
static_cast<pcl::uint8_t
> (r);
277 result.g =
static_cast<pcl::uint8_t
> (g);
278 result.b =
static_cast<pcl::uint8_t
> (b);
283 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowNonDense (
int i,
int j)
287 float r = 0, g = 0, b = 0;
288 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
294 result.x += (*input_) (l,j).x * kernel_[k]; result.y += (*input_) (l,j).y * kernel_[k]; result.z += (*input_) (l,j).z * kernel_[k];
295 r+= kernel_[k] *
static_cast<float> ((*input_) (l,j).r);
296 g+= kernel_[k] *
static_cast<float> ((*input_) (l,j).g);
297 b+= kernel_[k] *
static_cast<float> ((*input_) (l,j).b);
298 weight += kernel_[k];
303 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
307 r*= weight; g*= weight; b*= weight;
308 result.x*= weight; result.y*= weight; result.z*= weight;
309 result.r =
static_cast<pcl::uint8_t
> (r);
310 result.g =
static_cast<pcl::uint8_t
> (g);
311 result.b =
static_cast<pcl::uint8_t
> (b);
317 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColNonDense (
int i,
int j)
321 float r = 0, g = 0, b = 0;
322 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
328 result.x += (*input_) (i,l).x * kernel_[k]; result.y += (*input_) (i,l).y * kernel_[k]; result.z += (*input_) (i,l).z * kernel_[k];
329 r+= kernel_[k] *
static_cast<float> ((*input_) (i,l).r);
330 g+= kernel_[k] *
static_cast<float> ((*input_) (i,l).g);
331 b+= kernel_[k] *
static_cast<float> ((*input_) (i,l).b);
336 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
340 r*= weight; g*= weight; b*= weight;
341 result.x*= weight; result.y*= weight; result.z*= weight;
342 result.r =
static_cast<pcl::uint8_t
> (r);
343 result.g =
static_cast<pcl::uint8_t
> (g);
344 result.b =
static_cast<pcl::uint8_t
> (b);
351 Convolution<pcl::RGB, pcl::RGB>::convolveOneRowDense (
int i,
int j)
354 float r = 0, g = 0, b = 0;
355 for (
int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
357 r += kernel_[k] *
static_cast<float> ((*input_) (l,j).r);
358 g += kernel_[k] *
static_cast<float> ((*input_) (l,j).g);
359 b += kernel_[k] *
static_cast<float> ((*input_) (l,j).b);
361 result.r =
static_cast<pcl::uint8_t
> (r);
362 result.g =
static_cast<pcl::uint8_t
> (g);
363 result.b =
static_cast<pcl::uint8_t
> (b);
368 Convolution<pcl::RGB, pcl::RGB>::convolveOneColDense (
int i,
int j)
371 float r = 0, g = 0, b = 0;
372 for (
int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
374 r += kernel_[k] *
static_cast<float> ((*input_) (i,l).r);
375 g += kernel_[k] *
static_cast<float> ((*input_) (i,l).g);
376 b += kernel_[k] *
static_cast<float> ((*input_) (i,l).b);
378 result.r =
static_cast<pcl::uint8_t
> (r);
379 result.g =
static_cast<pcl::uint8_t
> (g);
380 result.b =
static_cast<pcl::uint8_t
> (b);
385 Convolution<pcl::RGB, pcl::RGB>::convolveOneRowNonDense (
int i,
int j)
387 return (convolveOneRowDense (i,j));
391 Convolution<pcl::RGB, pcl::RGB>::convolveOneColNonDense (
int i,
int j)
393 return (convolveOneColDense (i,j));
399 p.r = 0; p.g = 0; p.b = 0;
404 template <
typename Po
intIn,
typename Po
intOut>
void
407 using namespace pcl::common;
409 int width = input_->width;
410 int height = input_->height;
411 int last = input_->width - half_width_;
412 if (input_->is_dense)
415 #pragma omp parallel for shared (output) num_threads (threads_)
417 for(
int j = 0; j < height; ++j)
419 for (
int i = 0; i < half_width_; ++i)
420 makeInfinite (output (i,j));
422 for (
int i = half_width_; i < last; ++i)
423 output (i,j) = convolveOneRowDense (i,j);
425 for (
int i = last; i < width; ++i)
426 makeInfinite (output (i,j));
432 #pragma omp parallel for shared (output) num_threads (threads_)
434 for(
int j = 0; j < height; ++j)
436 for (
int i = 0; i < half_width_; ++i)
437 makeInfinite (output (i,j));
439 for (
int i = half_width_; i < last; ++i)
440 output (i,j) = convolveOneRowNonDense (i,j);
442 for (
int i = last; i < width; ++i)
443 makeInfinite (output (i,j));
448 template <
typename Po
intIn,
typename Po
intOut>
void
451 using namespace pcl::common;
453 int width = input_->width;
454 int height = input_->height;
455 int last = input_->width - half_width_;
457 if (input_->is_dense)
460 #pragma omp parallel for shared (output) num_threads (threads_)
462 for(
int j = 0; j < height; ++j)
464 for (
int i = half_width_; i < last; ++i)
465 output (i,j) = convolveOneRowDense (i,j);
467 for (
int i = last; i < width; ++i)
468 output (i,j) = output (w, j);
470 for (
int i = 0; i < half_width_; ++i)
471 output (i,j) = output (half_width_, j);
477 #pragma omp parallel for shared (output) num_threads (threads_)
479 for(
int j = 0; j < height; ++j)
481 for (
int i = half_width_; i < last; ++i)
482 output (i,j) = convolveOneRowNonDense (i,j);
484 for (
int i = last; i < width; ++i)
485 output (i,j) = output (w, j);
487 for (
int i = 0; i < half_width_; ++i)
488 output (i,j) = output (half_width_, j);
493 template <
typename Po
intIn,
typename Po
intOut>
void
496 using namespace pcl::common;
498 int width = input_->width;
499 int height = input_->height;
500 int last = input_->width - half_width_;
502 if (input_->is_dense)
505 #pragma omp parallel for shared (output) num_threads (threads_)
507 for(
int j = 0; j < height; ++j)
509 for (
int i = half_width_; i < last; ++i)
510 output (i,j) = convolveOneRowDense (i,j);
512 for (
int i = last, l = 0; i < width; ++i, ++l)
513 output (i,j) = output (w-l, j);
515 for (
int i = 0; i < half_width_; ++i)
516 output (i,j) = output (half_width_+1-i, j);
522 #pragma omp parallel for shared (output) num_threads (threads_)
524 for(
int j = 0; j < height; ++j)
526 for (
int i = half_width_; i < last; ++i)
527 output (i,j) = convolveOneRowNonDense (i,j);
529 for (
int i = last, l = 0; i < width; ++i, ++l)
530 output (i,j) = output (w-l, j);
532 for (
int i = 0; i < half_width_; ++i)
533 output (i,j) = output (half_width_+1-i, j);
538 template <
typename Po
intIn,
typename Po
intOut>
void
541 using namespace pcl::common;
543 int width = input_->width;
544 int height = input_->height;
545 int last = input_->height - half_width_;
546 if (input_->is_dense)
549 #pragma omp parallel for shared (output) num_threads (threads_)
551 for(
int i = 0; i < width; ++i)
553 for (
int j = 0; j < half_width_; ++j)
554 makeInfinite (output (i,j));
556 for (
int j = half_width_; j < last; ++j)
557 output (i,j) = convolveOneColDense (i,j);
559 for (
int j = last; j < height; ++j)
560 makeInfinite (output (i,j));
566 #pragma omp parallel for shared (output) num_threads (threads_)
568 for(
int i = 0; i < width; ++i)
570 for (
int j = 0; j < half_width_; ++j)
571 makeInfinite (output (i,j));
573 for (
int j = half_width_; j < last; ++j)
574 output (i,j) = convolveOneColNonDense (i,j);
576 for (
int j = last; j < height; ++j)
577 makeInfinite (output (i,j));
582 template <
typename Po
intIn,
typename Po
intOut>
void
585 using namespace pcl::common;
587 int width = input_->width;
588 int height = input_->height;
589 int last = input_->height - half_width_;
591 if (input_->is_dense)
594 #pragma omp parallel for shared (output) num_threads (threads_)
596 for(
int i = 0; i < width; ++i)
598 for (
int j = half_width_; j < last; ++j)
599 output (i,j) = convolveOneColDense (i,j);
601 for (
int j = last; j < height; ++j)
602 output (i,j) = output (i,h);
604 for (
int j = 0; j < half_width_; ++j)
605 output (i,j) = output (i, half_width_);
611 #pragma omp parallel for shared (output) num_threads (threads_)
613 for(
int i = 0; i < width; ++i)
615 for (
int j = half_width_; j < last; ++j)
616 output (i,j) = convolveOneColNonDense (i,j);
618 for (
int j = last; j < height; ++j)
619 output (i,j) = output (i,h);
621 for (
int j = 0; j < half_width_; ++j)
622 output (i,j) = output (i,half_width_);
627 template <
typename Po
intIn,
typename Po
intOut>
void
630 using namespace pcl::common;
632 int width = input_->width;
633 int height = input_->height;
634 int last = input_->height - half_width_;
636 if (input_->is_dense)
639 #pragma omp parallel for shared (output) num_threads (threads_)
641 for(
int i = 0; i < width; ++i)
643 for (
int j = half_width_; j < last; ++j)
644 output (i,j) = convolveOneColDense (i,j);
646 for (
int j = last, l = 0; j < height; ++j, ++l)
647 output (i,j) = output (i,h-l);
649 for (
int j = 0; j < half_width_; ++j)
650 output (i,j) = output (i, half_width_+1-j);
656 #pragma omp parallel for shared (output) num_threads (threads_)
658 for(
int i = 0; i < width; ++i)
660 for (
int j = half_width_; j < last; ++j)
661 output (i,j) = convolveOneColNonDense (i,j);
663 for (
int j = last, l = 0; j < height; ++j, ++l)
664 output (i,j) = output (i,h-l);
666 for (
int j = 0; j < half_width_; ++j)
667 output (i,j) = output (i,half_width_+1-j);
672 #endif //PCL_FILTERS_CONVOLUTION_IMPL_HPP
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
void convolve_cols_mirror(PointCloudOut &output)
convolve cols and mirror borders
void convolve_cols_duplicate(PointCloudOut &output)
convolve cols and duplicate borders
void convolve(const Eigen::ArrayXf &h_kernel, const Eigen::ArrayXf &v_kernel, PointCloudOut &output)
Convolve point cloud with an horizontal kernel along rows then vertical kernel along columns : convol...
uint32_t width
The point cloud width (if organized as an image-structure).
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
A structure representing RGB color information.
PointCloudIn::Ptr PointCloudInPtr
void convolve_rows_duplicate(PointCloudOut &output)
convolve rows and duplicate borders
Convolution is a mathematical operation on two functions f and g, producing a third function that is ...
void resize(size_t n)
Resize the cloud.
void convolve_rows(PointCloudOut &output)
convolve rows and ignore borders
char const * what() const
void convolve_rows_mirror(PointCloudOut &output)
convolve rows and mirror borders
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Convolution()
Constructor.
void convolveRows(PointCloudOut &output)
Convolve a float image rows by a given kernel.
void convolveCols(PointCloudOut &output)
Convolve a float image columns by a given kernel.
void initCompute(PointCloudOut &output)
init compute is an internal method called before computation
A point structure representing Euclidean xyz coordinates, and the RGB color.
void makeInfinite(PointOut &p)
void convolve_cols(PointCloudOut &output)
convolve cols and ignore borders
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
uint32_t height
The point cloud height (if organized as an image-structure).