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, last) 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, last) 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, last) 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, last) 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, last) 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, last) 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, last) 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, last) 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, last) 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, last) 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, last) 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, last) 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
void makeInfinite(PointOut &p)
uint32_t height
The point cloud height (if organized as an image-structure).
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
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...
void convolve_cols_mirror(PointCloudOut &output)
convolve cols and mirror borders
void convolve_rows_mirror(PointCloudOut &output)
convolve rows and mirror borders
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
PointCloudIn::Ptr PointCloudInPtr
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
void initCompute(PointCloudOut &output)
init compute is an internal method called before computation
void convolve_cols_duplicate(PointCloudOut &output)
convolve cols and duplicate borders
void convolve_rows_duplicate(PointCloudOut &output)
convolve rows and duplicate borders
void convolve_cols(PointCloudOut &output)
convolve cols and ignore borders
void resize(size_t n)
Resize the cloud.
Convolution()
Constructor.
char const * what() const
A point structure representing Euclidean xyz coordinates, and the RGB color.
uint32_t width
The point cloud width (if organized as an image-structure).
void convolve_rows(PointCloudOut &output)
convolve rows and ignore borders
void convolveRows(PointCloudOut &output)
Convolve a float image rows by a given kernel.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
A structure representing RGB color information.
void convolveCols(PointCloudOut &output)
Convolve a float image columns by a given kernel.
Convolution is a mathematical operation on two functions f and g, producing a third function that is ...