Point Cloud Library (PCL) 1.12.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 , half_width_ ()
59 , kernel_width_ ()
60 , threads_ (1)
61{}
62
63template <typename PointIn, typename PointOut> void
65{
66 if (borders_policy_ != BORDERS_POLICY_IGNORE &&
67 borders_policy_ != BORDERS_POLICY_MIRROR &&
68 borders_policy_ != BORDERS_POLICY_DUPLICATE)
69 PCL_THROW_EXCEPTION (InitFailedException,
70 "[pcl::filters::Convolution::initCompute] unknown borders policy.");
71
72 if(kernel_.size () % 2 == 0)
73 PCL_THROW_EXCEPTION (InitFailedException,
74 "[pcl::filters::Convolution::initCompute] convolving element width must be odd.");
75
76 if (distance_threshold_ != std::numeric_limits<float>::infinity ())
77 distance_threshold_ *= static_cast<float> (kernel_.size () % 2) * distance_threshold_;
78
79 half_width_ = static_cast<int> (kernel_.size ()) / 2;
80 kernel_width_ = static_cast<int> (kernel_.size () - 1);
81
82 if (&(*input_) != &output)
83 {
84 if (output.height != input_->height || output.width != input_->width)
85 {
86 output.resize (input_->width * input_->height);
87 output.width = input_->width;
88 output.height = input_->height;
89 }
90 }
91 output.is_dense = input_->is_dense;
92}
93
94template <typename PointIn, typename PointOut> inline void
96{
97 try
98 {
99 initCompute (output);
100 switch (borders_policy_)
101 {
102 case BORDERS_POLICY_MIRROR : convolve_rows_mirror (output); break;
103 case BORDERS_POLICY_DUPLICATE : convolve_rows_duplicate (output); break;
104 case BORDERS_POLICY_IGNORE : convolve_rows (output);
105 }
106 }
107 catch (InitFailedException& e)
108 {
109 PCL_THROW_EXCEPTION (InitFailedException,
110 "[pcl::filters::Convolution::convolveRows] init failed " << e.what ());
111 }
112}
113
114template <typename PointIn, typename PointOut> inline void
116{
117 try
118 {
119 initCompute (output);
120 switch (borders_policy_)
121 {
122 case BORDERS_POLICY_MIRROR : convolve_cols_mirror (output); break;
123 case BORDERS_POLICY_DUPLICATE : convolve_cols_duplicate (output); break;
124 case BORDERS_POLICY_IGNORE : convolve_cols (output);
125 }
126 }
127 catch (InitFailedException& e)
128 {
129 PCL_THROW_EXCEPTION (InitFailedException,
130 "[pcl::filters::Convolution::convolveCols] init failed " << e.what ());
131 }
132}
133
134template <typename PointIn, typename PointOut> inline void
135Convolution<PointIn, PointOut>::convolve (const Eigen::ArrayXf& h_kernel,
136 const Eigen::ArrayXf& v_kernel,
137 PointCloud<PointOut>& output)
138{
139 try
140 {
142 setKernel (h_kernel);
143 convolveRows (*tmp);
144 setInputCloud (tmp);
145 setKernel (v_kernel);
146 convolveCols (output);
147 }
148 catch (InitFailedException& e)
149 {
150 PCL_THROW_EXCEPTION (InitFailedException,
151 "[pcl::filters::Convolution::convolve] init failed " << e.what ());
152 }
153}
154
155template <typename PointIn, typename PointOut> inline void
157{
158 try
159 {
161 convolveRows (*tmp);
162 setInputCloud (tmp);
163 convolveCols (output);
164 }
165 catch (InitFailedException& e)
166 {
167 PCL_THROW_EXCEPTION (InitFailedException,
168 "[pcl::filters::Convolution::convolve] init failed " << e.what ());
169 }
170}
171
172template <typename PointIn, typename PointOut> inline PointOut
174{
175 using namespace pcl::common;
176 PointOut result;
177 for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
178 result+= (*input_) (l,j) * kernel_[k];
179 return (result);
180}
181
182template <typename PointIn, typename PointOut> inline PointOut
184{
185 using namespace pcl::common;
186 PointOut result;
187 for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
188 result+= (*input_) (i,l) * kernel_[k];
189 return (result);
190}
191
192template <typename PointIn, typename PointOut> inline PointOut
194{
195 using namespace pcl::common;
196 PointOut result;
197 float weight = 0;
198 for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
199 {
200 if (!isFinite ((*input_) (l,j)))
201 continue;
202 if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_)
203 {
204 result+= (*input_) (l,j) * kernel_[k];
205 weight += kernel_[k];
206 }
207 }
208 if (weight == 0)
209 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
210 else
211 {
212 weight = 1.f/weight;
213 result*= weight;
214 }
215 return (result);
216}
217
218template <typename PointIn, typename PointOut> inline PointOut
220{
221 using namespace pcl::common;
222 PointOut result;
223 float weight = 0;
224 for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
225 {
226 if (!isFinite ((*input_) (i,l)))
227 continue;
228 if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_)
229 {
230 result+= (*input_) (i,l) * kernel_[k];
231 weight += kernel_[k];
232 }
233 }
234 if (weight == 0)
235 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
236 else
237 {
238 weight = 1.f/weight;
239 result*= weight;
240 }
241 return (result);
242}
243
244template<> pcl::PointXYZRGB
246
247template<> pcl::PointXYZRGB
249
250template<> pcl::PointXYZRGB
252
253template<> pcl::PointXYZRGB
255
256template<> pcl::RGB
258
259template<> pcl::RGB
261
262template<> inline pcl::RGB
264{
265 return (convolveOneRowDense (i,j));
266}
267
268template<> inline pcl::RGB
270{
271 return (convolveOneColDense (i,j));
272}
273
274template<> inline void
276{
277 p.r = 0; p.g = 0; p.b = 0;
278}
279
280template <typename PointIn, typename PointOut> void
282{
283 using namespace pcl::common;
284
285 int width = input_->width;
286 int height = input_->height;
287 int last = input_->width - half_width_;
288 if (input_->is_dense)
289 {
290#pragma omp parallel for \
291 default(none) \
292 shared(height, last, output, width) \
293 num_threads(threads_)
294 for(int j = 0; j < height; ++j)
295 {
296 for (int i = 0; i < half_width_; ++i)
297 makeInfinite (output (i,j));
298
299 for (int i = half_width_; i < last; ++i)
300 output (i,j) = convolveOneRowDense (i,j);
301
302 for (int i = last; i < width; ++i)
303 makeInfinite (output (i,j));
304 }
305 }
306 else
307 {
308#pragma omp parallel for \
309 default(none) \
310 shared(height, last, output, width) \
311 num_threads(threads_)
312 for(int j = 0; j < height; ++j)
313 {
314 for (int i = 0; i < half_width_; ++i)
315 makeInfinite (output (i,j));
316
317 for (int i = half_width_; i < last; ++i)
318 output (i,j) = convolveOneRowNonDense (i,j);
319
320 for (int i = last; i < width; ++i)
321 makeInfinite (output (i,j));
322 }
323 }
324}
325
326template <typename PointIn, typename PointOut> void
328{
329 using namespace pcl::common;
330
331 int width = input_->width;
332 int height = input_->height;
333 int last = input_->width - half_width_;
334 int w = last - 1;
335 if (input_->is_dense)
336 {
337#pragma omp parallel for \
338 default(none) \
339 shared(height, last, output, w, width) \
340 num_threads(threads_)
341 for(int j = 0; j < height; ++j)
342 {
343 for (int i = half_width_; i < last; ++i)
344 output (i,j) = convolveOneRowDense (i,j);
345
346 for (int i = last; i < width; ++i)
347 output (i,j) = output (w, j);
348
349 for (int i = 0; i < half_width_; ++i)
350 output (i,j) = output (half_width_, j);
351 }
352 }
353 else
354 {
355#pragma omp parallel for \
356 default(none) \
357 shared(height, last, output, w, width) \
358 num_threads(threads_)
359 for(int j = 0; j < height; ++j)
360 {
361 for (int i = half_width_; i < last; ++i)
362 output (i,j) = convolveOneRowNonDense (i,j);
363
364 for (int i = last; i < width; ++i)
365 output (i,j) = output (w, j);
366
367 for (int i = 0; i < half_width_; ++i)
368 output (i,j) = output (half_width_, j);
369 }
370 }
371}
372
373template <typename PointIn, typename PointOut> void
375{
376 using namespace pcl::common;
377
378 int width = input_->width;
379 int height = input_->height;
380 int last = input_->width - half_width_;
381 int w = last - 1;
382 if (input_->is_dense)
383 {
384#pragma omp parallel for \
385 default(none) \
386 shared(height, last, output, w, width) \
387 num_threads(threads_)
388 for(int j = 0; j < height; ++j)
389 {
390 for (int i = half_width_; i < last; ++i)
391 output (i,j) = convolveOneRowDense (i,j);
392
393 for (int i = last, l = 0; i < width; ++i, ++l)
394 output (i,j) = output (w-l, j);
395
396 for (int i = 0; i < half_width_; ++i)
397 output (i,j) = output (half_width_+1-i, j);
398 }
399 }
400 else
401 {
402#pragma omp parallel for \
403 default(none) \
404 shared(height, last, output, w, width) \
405 num_threads(threads_)
406 for(int j = 0; j < height; ++j)
407 {
408 for (int i = half_width_; i < last; ++i)
409 output (i,j) = convolveOneRowNonDense (i,j);
410
411 for (int i = last, l = 0; i < width; ++i, ++l)
412 output (i,j) = output (w-l, j);
413
414 for (int i = 0; i < half_width_; ++i)
415 output (i,j) = output (half_width_+1-i, j);
416 }
417 }
418}
419
420template <typename PointIn, typename PointOut> void
422{
423 using namespace pcl::common;
424
425 int width = input_->width;
426 int height = input_->height;
427 int last = input_->height - half_width_;
428 if (input_->is_dense)
429 {
430#pragma omp parallel for \
431 default(none) \
432 shared(height, last, output, width) \
433 num_threads(threads_)
434 for(int i = 0; i < width; ++i)
435 {
436 for (int j = 0; j < half_width_; ++j)
437 makeInfinite (output (i,j));
438
439 for (int j = half_width_; j < last; ++j)
440 output (i,j) = convolveOneColDense (i,j);
441
442 for (int j = last; j < height; ++j)
443 makeInfinite (output (i,j));
444 }
445 }
446 else
447 {
448#pragma omp parallel for \
449 default(none) \
450 shared(height, last, output, width) \
451 num_threads(threads_)
452 for(int i = 0; i < width; ++i)
453 {
454 for (int j = 0; j < half_width_; ++j)
455 makeInfinite (output (i,j));
456
457 for (int j = half_width_; j < last; ++j)
458 output (i,j) = convolveOneColNonDense (i,j);
459
460 for (int j = last; j < height; ++j)
461 makeInfinite (output (i,j));
462 }
463 }
464}
465
466template <typename PointIn, typename PointOut> void
468{
469 using namespace pcl::common;
470
471 int width = input_->width;
472 int height = input_->height;
473 int last = input_->height - half_width_;
474 int h = last -1;
475 if (input_->is_dense)
476 {
477#pragma omp parallel for \
478 default(none) \
479 shared(h, height, last, output, width) \
480 num_threads(threads_)
481 for(int i = 0; i < width; ++i)
482 {
483 for (int j = half_width_; j < last; ++j)
484 output (i,j) = convolveOneColDense (i,j);
485
486 for (int j = last; j < height; ++j)
487 output (i,j) = output (i,h);
488
489 for (int j = 0; j < half_width_; ++j)
490 output (i,j) = output (i, half_width_);
491 }
492 }
493 else
494 {
495#pragma omp parallel for \
496 default(none) \
497 shared(h, height, last, output, width) \
498 num_threads(threads_)
499 for(int i = 0; i < width; ++i)
500 {
501 for (int j = half_width_; j < last; ++j)
502 output (i,j) = convolveOneColNonDense (i,j);
503
504 for (int j = last; j < height; ++j)
505 output (i,j) = output (i,h);
506
507 for (int j = 0; j < half_width_; ++j)
508 output (i,j) = output (i,half_width_);
509 }
510 }
511}
512
513template <typename PointIn, typename PointOut> void
515{
516 using namespace pcl::common;
517
518 int width = input_->width;
519 int height = input_->height;
520 int last = input_->height - half_width_;
521 int h = last -1;
522 if (input_->is_dense)
523 {
524#pragma omp parallel for \
525 default(none) \
526 shared(h, height, last, output, width) \
527 num_threads(threads_)
528 for(int i = 0; i < width; ++i)
529 {
530 for (int j = half_width_; j < last; ++j)
531 output (i,j) = convolveOneColDense (i,j);
532
533 for (int j = last, l = 0; j < height; ++j, ++l)
534 output (i,j) = output (i,h-l);
535
536 for (int j = 0; j < half_width_; ++j)
537 output (i,j) = output (i, half_width_+1-j);
538 }
539 }
540 else
541 {
542#pragma omp parallel for \
543 default(none) \
544 shared(h, height, last, output, width) \
545 num_threads(threads_)
546 for(int i = 0; i < width; ++i)
547 {
548 for (int j = half_width_; j < last; ++j)
549 output (i,j) = convolveOneColNonDense (i,j);
550
551 for (int j = last, l = 0; j < height; ++j, ++l)
552 output (i,j) = output (i,h-l);
553
554 for (int j = 0; j < half_width_; ++j)
555 output (i,j) = output (i,half_width_+1-j);
556 }
557 }
558}
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.