Point Cloud Library (PCL)  1.11.1-dev
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>
44 #include <pcl/common/distances.h>
45 #include <pcl/common/point_tests.h> // for pcl::isFinite
46 
47 
48 namespace pcl
49 {
50 namespace filters
51 {
52 
53 template <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 
63 template <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 
94 template <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 
114 template <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 
134 template <typename PointIn, typename PointOut> inline void
135 Convolution<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 
155 template <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 
172 template <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 
182 template <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 
192 template <typename PointIn, typename PointOut> inline PointOut
193 Convolution<PointIn, PointOut>::convolveOneRowNonDense (int i, int j)
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 
218 template <typename PointIn, typename PointOut> inline PointOut
219 Convolution<PointIn, PointOut>::convolveOneColNonDense (int i, int j)
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 
244 template<> pcl::PointXYZRGB
245 PCL_EXPORTS Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowDense (int i, int j);
246 
247 template<> pcl::PointXYZRGB
248 PCL_EXPORTS Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColDense (int i, int j);
249 
250 template<> pcl::PointXYZRGB
251 PCL_EXPORTS Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowNonDense (int i, int j);
252 
253 template<> pcl::PointXYZRGB
254 PCL_EXPORTS Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColNonDense (int i, int j);
255 
256 template<> pcl::RGB
257 PCL_EXPORTS Convolution<pcl::RGB, pcl::RGB>::convolveOneRowDense (int i, int j);
258 
259 template<> pcl::RGB
260 PCL_EXPORTS Convolution<pcl::RGB, pcl::RGB>::convolveOneColDense (int i, int j);
261 
262 template<> inline pcl::RGB
263 Convolution<pcl::RGB, pcl::RGB>::convolveOneRowNonDense (int i, int j)
264 {
265  return (convolveOneRowDense (i,j));
266 }
267 
268 template<> inline pcl::RGB
269 Convolution<pcl::RGB, pcl::RGB>::convolveOneColNonDense (int i, int j)
270 {
271  return (convolveOneColDense (i,j));
272 }
273 
274 template<> inline void
276 {
277  p.r = 0; p.g = 0; p.b = 0;
278 }
279 
280 template <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 
326 template <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 
373 template <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 
420 template <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 
466 template <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 
513 template <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 
pcl::filters::Convolution::convolveCols
void convolveCols(PointCloudOut &output)
Convolve a float image columns by a given kernel.
Definition: convolution.hpp:115
pcl
Definition: convolution.h:46
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::filters::Convolution::convolve
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...
Definition: convolution.hpp:135
pcl::isFinite
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
pcl::filters::Convolution::convolve_cols
void convolve_cols(PointCloudOut &output)
convolve cols and ignore borders
Definition: convolution.hpp:421
pcl::common
Definition: generate.h:49
pcl::filters::Convolution::convolve_rows_mirror
void convolve_rows_mirror(PointCloudOut &output)
convolve rows and mirror borders
Definition: convolution.hpp:374
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::filters::Convolution::initCompute
void initCompute(PointCloudOut &output)
init compute is an internal method called before computation
Definition: convolution.hpp:64
pcl::filters::Convolution
Convolution is a mathematical operation on two functions f and g, producing a third function that is ...
Definition: convolution.h:72
pcl::filters::Convolution::convolve_rows
void convolve_rows(PointCloudOut &output)
convolve rows and ignore borders
Definition: convolution.hpp:281
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::filters::Convolution::makeInfinite
void makeInfinite(PointOut &p)
Definition: convolution.h:224
pcl::squaredEuclideanDistance
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:182
pcl::filters::Convolution::Convolution
Convolution()
Constructor.
Definition: convolution.hpp:54
pcl::filters::Convolution::convolve_cols_mirror
void convolve_cols_mirror(PointCloudOut &output)
convolve cols and mirror borders
Definition: convolution.hpp:514
pcl::RGB
A structure representing RGB color information.
Definition: point_types.hpp:345
pcl::filters::Convolution::convolve_rows_duplicate
void convolve_rows_duplicate(PointCloudOut &output)
convolve rows and duplicate borders
Definition: convolution.hpp:327
pcl::filters::Convolution::convolveRows
void convolveRows(PointCloudOut &output)
Convolve a float image rows by a given kernel.
Definition: convolution.hpp:95
pcl::Convolution
A 2D convolution class.
Definition: convolution.h:60
pcl::filters::Convolution::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition: convolution.h:76
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:397
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::InitFailedException
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:193
pcl::filters::Convolution::convolve_cols_duplicate
void convolve_cols_duplicate(PointCloudOut &output)
convolve cols and duplicate borders
Definition: convolution.hpp:467
distances.h
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:323