Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
convolution.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#pragma once
42
43#include <pcl/pcl_config.h>
45#include <pcl/common/point_tests.h> // for pcl::isFinite
46
47
48namespace pcl
49{
50namespace filters
51{
52
53template <typename PointIn, typename PointOut>
55 : borders_policy_ (BORDERS_POLICY_IGNORE)
56 , distance_threshold_ (std::numeric_limits<float>::infinity ())
57 , input_ ()
58{}
59
60template <typename PointIn, typename PointOut> void
62{
63 if (borders_policy_ != BORDERS_POLICY_IGNORE &&
64 borders_policy_ != BORDERS_POLICY_MIRROR &&
65 borders_policy_ != BORDERS_POLICY_DUPLICATE)
66 PCL_THROW_EXCEPTION (InitFailedException,
67 "[pcl::filters::Convolution::initCompute] unknown borders policy.");
68
69 if(kernel_.size () % 2 == 0)
70 PCL_THROW_EXCEPTION (InitFailedException,
71 "[pcl::filters::Convolution::initCompute] convolving element width must be odd.");
72
73 if (distance_threshold_ != std::numeric_limits<float>::infinity ())
74 distance_threshold_ *= static_cast<float> (kernel_.size () % 2) * distance_threshold_;
75
76 half_width_ = static_cast<int> (kernel_.size ()) / 2;
77 kernel_width_ = static_cast<int> (kernel_.size () - 1);
78
79 if (&(*input_) != &output)
80 {
81 if (output.height != input_->height || output.width != input_->width)
82 {
83 output.resize (input_->width * input_->height);
84 output.width = input_->width;
85 output.height = input_->height;
86 }
87 }
88 output.is_dense = input_->is_dense;
89}
90
91template <typename PointIn, typename PointOut> inline void
93{
94 try
95 {
96 initCompute (output);
97 switch (borders_policy_)
98 {
99 case BORDERS_POLICY_MIRROR : convolve_rows_mirror (output); break;
100 case BORDERS_POLICY_DUPLICATE : convolve_rows_duplicate (output); break;
101 case BORDERS_POLICY_IGNORE : convolve_rows (output);
102 }
103 }
104 catch (InitFailedException& e)
105 {
106 PCL_THROW_EXCEPTION (InitFailedException,
107 "[pcl::filters::Convolution::convolveRows] init failed " << e.what ());
108 }
109}
110
111template <typename PointIn, typename PointOut> inline void
113{
114 try
115 {
116 initCompute (output);
117 switch (borders_policy_)
118 {
119 case BORDERS_POLICY_MIRROR : convolve_cols_mirror (output); break;
120 case BORDERS_POLICY_DUPLICATE : convolve_cols_duplicate (output); break;
121 case BORDERS_POLICY_IGNORE : convolve_cols (output);
122 }
123 }
124 catch (InitFailedException& e)
125 {
126 PCL_THROW_EXCEPTION (InitFailedException,
127 "[pcl::filters::Convolution::convolveCols] init failed " << e.what ());
128 }
129}
130
131template <typename PointIn, typename PointOut> inline void
132Convolution<PointIn, PointOut>::convolve (const Eigen::ArrayXf& h_kernel,
133 const Eigen::ArrayXf& v_kernel,
134 PointCloud<PointOut>& output)
135{
136 try
137 {
139 setKernel (h_kernel);
140 convolveRows (*tmp);
141 setInputCloud (tmp);
142 setKernel (v_kernel);
143 convolveCols (output);
144 }
145 catch (InitFailedException& e)
146 {
147 PCL_THROW_EXCEPTION (InitFailedException,
148 "[pcl::filters::Convolution::convolve] init failed " << e.what ());
149 }
150}
151
152template <typename PointIn, typename PointOut> inline void
154{
155 try
156 {
158 convolveRows (*tmp);
159 setInputCloud (tmp);
160 convolveCols (output);
161 }
162 catch (InitFailedException& e)
163 {
164 PCL_THROW_EXCEPTION (InitFailedException,
165 "[pcl::filters::Convolution::convolve] init failed " << e.what ());
166 }
167}
168
169template <typename PointIn, typename PointOut> inline PointOut
171{
172 using namespace pcl::common;
173 PointOut result;
174 for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
175 result+= (*input_) (l,j) * kernel_[k];
176 return (result);
177}
178
179template <typename PointIn, typename PointOut> inline PointOut
181{
182 using namespace pcl::common;
183 PointOut result;
184 for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
185 result+= (*input_) (i,l) * kernel_[k];
186 return (result);
187}
188
189template <typename PointIn, typename PointOut> inline PointOut
191{
192 using namespace pcl::common;
193 PointOut result;
194 float weight = 0;
195 for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
196 {
197 if (!isFinite ((*input_) (l,j)))
198 continue;
199 if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_)
200 {
201 result+= (*input_) (l,j) * kernel_[k];
202 weight += kernel_[k];
203 }
204 }
205 if (weight == 0)
206 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
207 else
208 {
209 weight = 1.f/weight;
210 result*= weight;
211 }
212 return (result);
213}
214
215template <typename PointIn, typename PointOut> inline PointOut
217{
218 using namespace pcl::common;
219 PointOut result;
220 float weight = 0;
221 for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
222 {
223 if (!isFinite ((*input_) (i,l)))
224 continue;
225 if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_)
226 {
227 result+= (*input_) (i,l) * kernel_[k];
228 weight += kernel_[k];
229 }
230 }
231 if (weight == 0)
232 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
233 else
234 {
235 weight = 1.f/weight;
236 result*= weight;
237 }
238 return (result);
239}
240
241template<> pcl::PointXYZRGB
243
244template<> pcl::PointXYZRGB
246
247template<> pcl::PointXYZRGB
249
250template<> pcl::PointXYZRGB
252
253template<> pcl::RGB
255
256template<> pcl::RGB
258
259template<> inline pcl::RGB
261{
262 return (convolveOneRowDense (i,j));
263}
264
265template<> inline pcl::RGB
267{
268 return (convolveOneColDense (i,j));
269}
270
271template<> inline void
273{
274 p.r = 0; p.g = 0; p.b = 0;
275}
276
277template <typename PointIn, typename PointOut> void
279{
280 using namespace pcl::common;
281
282 int width = input_->width;
283 int height = input_->height;
284 int last = input_->width - half_width_;
285 if (input_->is_dense)
286 {
287#pragma omp parallel for \
288 default(none) \
289 shared(height, last, output, width) \
290 num_threads(threads_)
291 for(int j = 0; j < height; ++j)
292 {
293 for (int i = 0; i < half_width_; ++i)
294 makeInfinite (output (i,j));
295
296 for (int i = half_width_; i < last; ++i)
297 output (i,j) = convolveOneRowDense (i,j);
298
299 for (int i = last; i < width; ++i)
300 makeInfinite (output (i,j));
301 }
302 }
303 else
304 {
305#pragma omp parallel for \
306 default(none) \
307 shared(height, last, output, width) \
308 num_threads(threads_)
309 for(int j = 0; j < height; ++j)
310 {
311 for (int i = 0; i < half_width_; ++i)
312 makeInfinite (output (i,j));
313
314 for (int i = half_width_; i < last; ++i)
315 output (i,j) = convolveOneRowNonDense (i,j);
316
317 for (int i = last; i < width; ++i)
318 makeInfinite (output (i,j));
319 }
320 }
321}
322
323template <typename PointIn, typename PointOut> void
325{
326 using namespace pcl::common;
327
328 int width = input_->width;
329 int height = input_->height;
330 int last = input_->width - half_width_;
331 int w = last - 1;
332 if (input_->is_dense)
333 {
334#pragma omp parallel for \
335 default(none) \
336 shared(height, last, output, w, width) \
337 num_threads(threads_)
338 for(int j = 0; j < height; ++j)
339 {
340 for (int i = half_width_; i < last; ++i)
341 output (i,j) = convolveOneRowDense (i,j);
342
343 for (int i = last; i < width; ++i)
344 output (i,j) = output (w, j);
345
346 for (int i = 0; i < half_width_; ++i)
347 output (i,j) = output (half_width_, j);
348 }
349 }
350 else
351 {
352#pragma omp parallel for \
353 default(none) \
354 shared(height, last, output, w, width) \
355 num_threads(threads_)
356 for(int j = 0; j < height; ++j)
357 {
358 for (int i = half_width_; i < last; ++i)
359 output (i,j) = convolveOneRowNonDense (i,j);
360
361 for (int i = last; i < width; ++i)
362 output (i,j) = output (w, j);
363
364 for (int i = 0; i < half_width_; ++i)
365 output (i,j) = output (half_width_, j);
366 }
367 }
368}
369
370template <typename PointIn, typename PointOut> void
372{
373 using namespace pcl::common;
374
375 int width = input_->width;
376 int height = input_->height;
377 int last = input_->width - half_width_;
378 int w = last - 1;
379 if (input_->is_dense)
380 {
381#pragma omp parallel for \
382 default(none) \
383 shared(height, last, output, w, width) \
384 num_threads(threads_)
385 for(int j = 0; j < height; ++j)
386 {
387 for (int i = half_width_; i < last; ++i)
388 output (i,j) = convolveOneRowDense (i,j);
389
390 for (int i = last, l = 0; i < width; ++i, ++l)
391 output (i,j) = output (w-l, j);
392
393 for (int i = 0; i < half_width_; ++i)
394 output (i,j) = output (half_width_+1-i, j);
395 }
396 }
397 else
398 {
399#pragma omp parallel for \
400 default(none) \
401 shared(height, last, output, w, width) \
402 num_threads(threads_)
403 for(int j = 0; j < height; ++j)
404 {
405 for (int i = half_width_; i < last; ++i)
406 output (i,j) = convolveOneRowNonDense (i,j);
407
408 for (int i = last, l = 0; i < width; ++i, ++l)
409 output (i,j) = output (w-l, j);
410
411 for (int i = 0; i < half_width_; ++i)
412 output (i,j) = output (half_width_+1-i, j);
413 }
414 }
415}
416
417template <typename PointIn, typename PointOut> void
419{
420 using namespace pcl::common;
421
422 int width = input_->width;
423 int height = input_->height;
424 int last = input_->height - half_width_;
425 if (input_->is_dense)
426 {
427#pragma omp parallel for \
428 default(none) \
429 shared(height, last, output, width) \
430 num_threads(threads_)
431 for(int i = 0; i < width; ++i)
432 {
433 for (int j = 0; j < half_width_; ++j)
434 makeInfinite (output (i,j));
435
436 for (int j = half_width_; j < last; ++j)
437 output (i,j) = convolveOneColDense (i,j);
438
439 for (int j = last; j < height; ++j)
440 makeInfinite (output (i,j));
441 }
442 }
443 else
444 {
445#pragma omp parallel for \
446 default(none) \
447 shared(height, last, output, width) \
448 num_threads(threads_)
449 for(int i = 0; i < width; ++i)
450 {
451 for (int j = 0; j < half_width_; ++j)
452 makeInfinite (output (i,j));
453
454 for (int j = half_width_; j < last; ++j)
455 output (i,j) = convolveOneColNonDense (i,j);
456
457 for (int j = last; j < height; ++j)
458 makeInfinite (output (i,j));
459 }
460 }
461}
462
463template <typename PointIn, typename PointOut> void
465{
466 using namespace pcl::common;
467
468 int width = input_->width;
469 int height = input_->height;
470 int last = input_->height - half_width_;
471 int h = last -1;
472 if (input_->is_dense)
473 {
474#pragma omp parallel for \
475 default(none) \
476 shared(h, height, last, output, width) \
477 num_threads(threads_)
478 for(int i = 0; i < width; ++i)
479 {
480 for (int j = half_width_; j < last; ++j)
481 output (i,j) = convolveOneColDense (i,j);
482
483 for (int j = last; j < height; ++j)
484 output (i,j) = output (i,h);
485
486 for (int j = 0; j < half_width_; ++j)
487 output (i,j) = output (i, half_width_);
488 }
489 }
490 else
491 {
492#pragma omp parallel for \
493 default(none) \
494 shared(h, height, last, output, width) \
495 num_threads(threads_)
496 for(int i = 0; i < width; ++i)
497 {
498 for (int j = half_width_; j < last; ++j)
499 output (i,j) = convolveOneColNonDense (i,j);
500
501 for (int j = last; j < height; ++j)
502 output (i,j) = output (i,h);
503
504 for (int j = 0; j < half_width_; ++j)
505 output (i,j) = output (i,half_width_);
506 }
507 }
508}
509
510template <typename PointIn, typename PointOut> void
512{
513 using namespace pcl::common;
514
515 int width = input_->width;
516 int height = input_->height;
517 int last = input_->height - half_width_;
518 int h = last -1;
519 if (input_->is_dense)
520 {
521#pragma omp parallel for \
522 default(none) \
523 shared(h, height, last, output, width) \
524 num_threads(threads_)
525 for(int i = 0; i < width; ++i)
526 {
527 for (int j = half_width_; j < last; ++j)
528 output (i,j) = convolveOneColDense (i,j);
529
530 for (int j = last, l = 0; j < height; ++j, ++l)
531 output (i,j) = output (i,h-l);
532
533 for (int j = 0; j < half_width_; ++j)
534 output (i,j) = output (i, half_width_+1-j);
535 }
536 }
537 else
538 {
539#pragma omp parallel for \
540 default(none) \
541 shared(h, height, last, output, width) \
542 num_threads(threads_)
543 for(int i = 0; i < width; ++i)
544 {
545 for (int j = half_width_; j < last; ++j)
546 output (i,j) = convolveOneColNonDense (i,j);
547
548 for (int j = last, l = 0; j < height; ++j, ++l)
549 output (i,j) = output (i,h-l);
550
551 for (int j = 0; j < half_width_; ++j)
552 output (i,j) = output (i,half_width_+1-j);
553 }
554 }
555}
556
557#define PCL_INSTANTIATE_Convolution(Tin, Tout) \
558 template class PCL_EXPORTS Convolution<Tin, Tout>;
559
560} // namespace filters
561} // namespace pcl
562
A 2D convolution class.
Definition convolution.h:60
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition exceptions.h:194
bool initCompute()
This method should get called before starting the actual computation.
Definition pcl_base.hpp:138
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
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).
Convolution is a mathematical operation on two functions f and g, producing a third function that is ...
Definition convolution.h:73
typename PointCloudIn::Ptr PointCloudInPtr
Definition convolution.h:76
void convolveCols(PointCloudOut &output)
Convolve a float image columns by a given kernel.
void convolve_rows_mirror(PointCloudOut &output)
convolve rows and mirror borders
void makeInfinite(PointOut &p)
void convolve_cols_duplicate(PointCloudOut &output)
convolve cols and duplicate borders
void convolve_rows_duplicate(PointCloudOut &output)
convolve rows and duplicate borders
void convolveRows(PointCloudOut &output)
Convolve a float image rows by a given kernel.
void convolve_cols(PointCloudOut &output)
convolve cols and ignore borders
void convolve_cols_mirror(PointCloudOut &output)
convolve cols and mirror borders
void convolve_rows(PointCloudOut &output)
convolve rows and ignore 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...
Define standard C methods to do distance calculations.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition distances.h:182
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
A point structure representing Euclidean xyz coordinates, and the RGB color.
A structure representing RGB color information.