Point Cloud Library (PCL)  1.7.0
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  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_FILTERS_CONVOLUTION_IMPL_HPP
41 #define PCL_FILTERS_CONVOLUTION_IMPL_HPP
42 
43 #include <pcl/pcl_config.h>
44 #include <pcl/common/distances.h>
45 
46 template <typename PointIn, typename PointOut>
48  : borders_policy_ (BORDERS_POLICY_IGNORE)
49  , distance_threshold_ (std::numeric_limits<float>::infinity ())
50  , input_ ()
51  , kernel_ ()
52  , half_width_ ()
53  , kernel_width_ ()
54  , threads_ (1)
55 {}
56 
57 template <typename PointIn, typename PointOut> void
59 {
60  if (borders_policy_ != BORDERS_POLICY_IGNORE &&
61  borders_policy_ != BORDERS_POLICY_MIRROR &&
62  borders_policy_ != BORDERS_POLICY_DUPLICATE)
63  PCL_THROW_EXCEPTION (InitFailedException,
64  "[pcl::filters::Convolution::initCompute] unknown borders policy.");
65 
66  if(kernel_.size () % 2 == 0)
67  PCL_THROW_EXCEPTION (InitFailedException,
68  "[pcl::filters::Convolution::initCompute] convolving element width must be odd.");
69 
70  if (distance_threshold_ != std::numeric_limits<float>::infinity ())
71  distance_threshold_ *= static_cast<float> (kernel_.size () % 2) * distance_threshold_;
72 
73  half_width_ = static_cast<int> (kernel_.size ()) / 2;
74  kernel_width_ = static_cast<int> (kernel_.size () - 1);
75 
76  if (&(*input_) != &output)
77  {
78  if (output.height != input_->height || output.width != input_->width)
79  {
80  output.resize (input_->width * input_->height);
81  output.width = input_->width;
82  output.height = input_->height;
83  }
84  }
85  output.is_dense = input_->is_dense;
86 }
87 
88 template <typename PointIn, typename PointOut> inline void
90 {
91  try
92  {
93  initCompute (output);
94  switch (borders_policy_)
95  {
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);
99  }
100  }
101  catch (InitFailedException& e)
102  {
103  PCL_THROW_EXCEPTION (InitFailedException,
104  "[pcl::filters::Convolution::convolveRows] init failed " << e.what ());
105  }
106 }
107 
108 template <typename PointIn, typename PointOut> inline void
110 {
111  try
112  {
113  initCompute (output);
114  switch (borders_policy_)
115  {
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);
119  }
120  }
121  catch (InitFailedException& e)
122  {
123  PCL_THROW_EXCEPTION (InitFailedException,
124  "[pcl::filters::Convolution::convolveCols] init failed " << e.what ());
125  }
126 }
127 
128 template <typename PointIn, typename PointOut> inline void
130  const Eigen::ArrayXf& v_kernel,
131  PointCloud<PointOut>& output)
132 {
133  try
134  {
136  setKernel (h_kernel);
137  convolveRows (*tmp);
138  setInputCloud (tmp);
139  setKernel (v_kernel);
140  convolveCols (output);
141  }
142  catch (InitFailedException& e)
143  {
144  PCL_THROW_EXCEPTION (InitFailedException,
145  "[pcl::filters::Convolution::convolve] init failed " << e.what ());
146  }
147 }
148 
149 template <typename PointIn, typename PointOut> inline void
151 {
152  try
153  {
155  convolveRows (*tmp);
156  setInputCloud (tmp);
157  convolveCols (output);
158  }
159  catch (InitFailedException& e)
160  {
161  PCL_THROW_EXCEPTION (InitFailedException,
162  "[pcl::filters::Convolution::convolve] init failed " << e.what ());
163  }
164 }
165 
166 template <typename PointIn, typename PointOut> inline PointOut
168 {
169  using namespace pcl::common;
170  PointOut result;
171  for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
172  result+= (*input_) (l,j) * kernel_[k];
173  return (result);
174 }
175 
176 template <typename PointIn, typename PointOut> inline PointOut
178 {
179  using namespace pcl::common;
180  PointOut result;
181  for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
182  result+= (*input_) (i,l) * kernel_[k];
183  return (result);
184 }
185 
186 template <typename PointIn, typename PointOut> inline PointOut
188 {
189  using namespace pcl::common;
190  PointOut result;
191  float weight = 0;
192  for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
193  {
194  if (!isFinite ((*input_) (l,j)))
195  continue;
196  if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_)
197  {
198  result+= (*input_) (l,j) * kernel_[k];
199  weight += kernel_[k];
200  }
201  }
202  if (weight == 0)
203  result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
204  else
205  {
206  weight = 1.f/weight;
207  result*= weight;
208  }
209  return (result);
210 }
211 
212 template <typename PointIn, typename PointOut> inline PointOut
214 {
215  using namespace pcl::common;
216  PointOut result;
217  float weight = 0;
218  for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
219  {
220  if (!isFinite ((*input_) (i,l)))
221  continue;
222  if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_)
223  {
224  result+= (*input_) (i,l) * kernel_[k];
225  weight += kernel_[k];
226  }
227  }
228  if (weight == 0)
229  result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
230  else
231  {
232  weight = 1.f/weight;
233  result*= weight;
234  }
235  return (result);
236 }
237 
238 namespace pcl
239 {
240  namespace filters
241  {
242  template<> pcl::PointXYZRGB
243  Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowDense (int i, int j)
244  {
245  pcl::PointXYZRGB result;
246  float r = 0, g = 0, b = 0;
247  for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
248  {
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);
255  }
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);
259  return (result);
260  }
261 
262  template<> pcl::PointXYZRGB
263  Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColDense (int i, int j)
264  {
265  pcl::PointXYZRGB result;
266  float r = 0, g = 0, b = 0;
267  for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
268  {
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);
275  }
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);
279  return (result);
280  }
281 
282  template<> pcl::PointXYZRGB
283  Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowNonDense (int i, int j)
284  {
285  pcl::PointXYZRGB result;
286  float weight = 0;
287  float r = 0, g = 0, b = 0;
288  for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
289  {
290  if (!isFinite ((*input_) (l,j)))
291  continue;
292  if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_)
293  {
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];
299  }
300  }
301 
302  if (weight == 0)
303  result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
304  else
305  {
306  weight = 1.f/weight;
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);
312  }
313  return (result);
314  }
315 
316  template<> pcl::PointXYZRGB
317  Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColNonDense (int i, int j)
318  {
319  pcl::PointXYZRGB result;
320  float weight = 0;
321  float r = 0, g = 0, b = 0;
322  for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
323  {
324  if (!isFinite ((*input_) (i,l)))
325  continue;
326  if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_)
327  {
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);
332  weight+= kernel_[k];
333  }
334  }
335  if (weight == 0)
336  result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN ();
337  else
338  {
339  weight = 1.f/weight;
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);
345  }
346  return (result);
347  }
348 
349  ///////////////////////////////////////////////////////////////////////////////////////////////
350  template<> pcl::RGB
351  Convolution<pcl::RGB, pcl::RGB>::convolveOneRowDense (int i, int j)
352  {
353  pcl::RGB result;
354  float r = 0, g = 0, b = 0;
355  for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l)
356  {
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);
360  }
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);
364  return (result);
365  }
366 
367  template<> pcl::RGB
368  Convolution<pcl::RGB, pcl::RGB>::convolveOneColDense (int i, int j)
369  {
370  pcl::RGB result;
371  float r = 0, g = 0, b = 0;
372  for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l)
373  {
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);
377  }
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);
381  return (result);
382  }
383 
384  template<> pcl::RGB
385  Convolution<pcl::RGB, pcl::RGB>::convolveOneRowNonDense (int i, int j)
386  {
387  return (convolveOneRowDense (i,j));
388  }
389 
390  template<> pcl::RGB
391  Convolution<pcl::RGB, pcl::RGB>::convolveOneColNonDense (int i, int j)
392  {
393  return (convolveOneColDense (i,j));
394  }
395 
396  template<> void
398  {
399  p.r = 0; p.g = 0; p.b = 0;
400  }
401  }
402 }
403 
404 template <typename PointIn, typename PointOut> void
406 {
407  using namespace pcl::common;
408 
409  int width = input_->width;
410  int height = input_->height;
411  int last = input_->width - half_width_;
412  if (input_->is_dense)
413  {
414 #ifdef _OPENMP
415 #pragma omp parallel for shared (output, last) num_threads (threads_)
416 #endif
417  for(int j = 0; j < height; ++j)
418  {
419  for (int i = 0; i < half_width_; ++i)
420  makeInfinite (output (i,j));
421 
422  for (int i = half_width_; i < last; ++i)
423  output (i,j) = convolveOneRowDense (i,j);
424 
425  for (int i = last; i < width; ++i)
426  makeInfinite (output (i,j));
427  }
428  }
429  else
430  {
431 #ifdef _OPENMP
432 #pragma omp parallel for shared (output, last) num_threads (threads_)
433 #endif
434  for(int j = 0; j < height; ++j)
435  {
436  for (int i = 0; i < half_width_; ++i)
437  makeInfinite (output (i,j));
438 
439  for (int i = half_width_; i < last; ++i)
440  output (i,j) = convolveOneRowNonDense (i,j);
441 
442  for (int i = last; i < width; ++i)
443  makeInfinite (output (i,j));
444  }
445  }
446 }
447 
448 template <typename PointIn, typename PointOut> void
450 {
451  using namespace pcl::common;
452 
453  int width = input_->width;
454  int height = input_->height;
455  int last = input_->width - half_width_;
456  int w = last - 1;
457  if (input_->is_dense)
458  {
459 #ifdef _OPENMP
460 #pragma omp parallel for shared (output, last) num_threads (threads_)
461 #endif
462  for(int j = 0; j < height; ++j)
463  {
464  for (int i = half_width_; i < last; ++i)
465  output (i,j) = convolveOneRowDense (i,j);
466 
467  for (int i = last; i < width; ++i)
468  output (i,j) = output (w, j);
469 
470  for (int i = 0; i < half_width_; ++i)
471  output (i,j) = output (half_width_, j);
472  }
473  }
474  else
475  {
476 #ifdef _OPENMP
477 #pragma omp parallel for shared (output, last) num_threads (threads_)
478 #endif
479  for(int j = 0; j < height; ++j)
480  {
481  for (int i = half_width_; i < last; ++i)
482  output (i,j) = convolveOneRowNonDense (i,j);
483 
484  for (int i = last; i < width; ++i)
485  output (i,j) = output (w, j);
486 
487  for (int i = 0; i < half_width_; ++i)
488  output (i,j) = output (half_width_, j);
489  }
490  }
491 }
492 
493 template <typename PointIn, typename PointOut> void
495 {
496  using namespace pcl::common;
497 
498  int width = input_->width;
499  int height = input_->height;
500  int last = input_->width - half_width_;
501  int w = last - 1;
502  if (input_->is_dense)
503  {
504 #ifdef _OPENMP
505 #pragma omp parallel for shared (output, last) num_threads (threads_)
506 #endif
507  for(int j = 0; j < height; ++j)
508  {
509  for (int i = half_width_; i < last; ++i)
510  output (i,j) = convolveOneRowDense (i,j);
511 
512  for (int i = last, l = 0; i < width; ++i, ++l)
513  output (i,j) = output (w-l, j);
514 
515  for (int i = 0; i < half_width_; ++i)
516  output (i,j) = output (half_width_+1-i, j);
517  }
518  }
519  else
520  {
521 #ifdef _OPENMP
522 #pragma omp parallel for shared (output, last) num_threads (threads_)
523 #endif
524  for(int j = 0; j < height; ++j)
525  {
526  for (int i = half_width_; i < last; ++i)
527  output (i,j) = convolveOneRowNonDense (i,j);
528 
529  for (int i = last, l = 0; i < width; ++i, ++l)
530  output (i,j) = output (w-l, j);
531 
532  for (int i = 0; i < half_width_; ++i)
533  output (i,j) = output (half_width_+1-i, j);
534  }
535  }
536 }
537 
538 template <typename PointIn, typename PointOut> void
540 {
541  using namespace pcl::common;
542 
543  int width = input_->width;
544  int height = input_->height;
545  int last = input_->height - half_width_;
546  if (input_->is_dense)
547  {
548 #ifdef _OPENMP
549 #pragma omp parallel for shared (output, last) num_threads (threads_)
550 #endif
551  for(int i = 0; i < width; ++i)
552  {
553  for (int j = 0; j < half_width_; ++j)
554  makeInfinite (output (i,j));
555 
556  for (int j = half_width_; j < last; ++j)
557  output (i,j) = convolveOneColDense (i,j);
558 
559  for (int j = last; j < height; ++j)
560  makeInfinite (output (i,j));
561  }
562  }
563  else
564  {
565 #ifdef _OPENMP
566 #pragma omp parallel for shared (output, last) num_threads (threads_)
567 #endif
568  for(int i = 0; i < width; ++i)
569  {
570  for (int j = 0; j < half_width_; ++j)
571  makeInfinite (output (i,j));
572 
573  for (int j = half_width_; j < last; ++j)
574  output (i,j) = convolveOneColNonDense (i,j);
575 
576  for (int j = last; j < height; ++j)
577  makeInfinite (output (i,j));
578  }
579  }
580 }
581 
582 template <typename PointIn, typename PointOut> void
584 {
585  using namespace pcl::common;
586 
587  int width = input_->width;
588  int height = input_->height;
589  int last = input_->height - half_width_;
590  int h = last -1;
591  if (input_->is_dense)
592  {
593 #ifdef _OPENMP
594 #pragma omp parallel for shared (output, last) num_threads (threads_)
595 #endif
596  for(int i = 0; i < width; ++i)
597  {
598  for (int j = half_width_; j < last; ++j)
599  output (i,j) = convolveOneColDense (i,j);
600 
601  for (int j = last; j < height; ++j)
602  output (i,j) = output (i,h);
603 
604  for (int j = 0; j < half_width_; ++j)
605  output (i,j) = output (i, half_width_);
606  }
607  }
608  else
609  {
610 #ifdef _OPENMP
611 #pragma omp parallel for shared (output, last) num_threads (threads_)
612 #endif
613  for(int i = 0; i < width; ++i)
614  {
615  for (int j = half_width_; j < last; ++j)
616  output (i,j) = convolveOneColNonDense (i,j);
617 
618  for (int j = last; j < height; ++j)
619  output (i,j) = output (i,h);
620 
621  for (int j = 0; j < half_width_; ++j)
622  output (i,j) = output (i,half_width_);
623  }
624  }
625 }
626 
627 template <typename PointIn, typename PointOut> void
629 {
630  using namespace pcl::common;
631 
632  int width = input_->width;
633  int height = input_->height;
634  int last = input_->height - half_width_;
635  int h = last -1;
636  if (input_->is_dense)
637  {
638 #ifdef _OPENMP
639 #pragma omp parallel for shared (output, last) num_threads (threads_)
640 #endif
641  for(int i = 0; i < width; ++i)
642  {
643  for (int j = half_width_; j < last; ++j)
644  output (i,j) = convolveOneColDense (i,j);
645 
646  for (int j = last, l = 0; j < height; ++j, ++l)
647  output (i,j) = output (i,h-l);
648 
649  for (int j = 0; j < half_width_; ++j)
650  output (i,j) = output (i, half_width_+1-j);
651  }
652  }
653  else
654  {
655 #ifdef _OPENMP
656 #pragma omp parallel for shared (output, last) num_threads (threads_)
657 #endif
658  for(int i = 0; i < width; ++i)
659  {
660  for (int j = half_width_; j < last; ++j)
661  output (i,j) = convolveOneColNonDense (i,j);
662 
663  for (int j = last, l = 0; j < height; ++j, ++l)
664  output (i,j) = output (i,h-l);
665 
666  for (int j = 0; j < half_width_; ++j)
667  output (i,j) = output (i,half_width_+1-j);
668  }
669  }
670 }
671 
672 #endif //PCL_FILTERS_CONVOLUTION_IMPL_HPP