Point Cloud Library (PCL)  1.14.0-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 {}
59 
60 template <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 
91 template <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 
111 template <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 
131 template <typename PointIn, typename PointOut> inline void
132 Convolution<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 
152 template <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 
169 template <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 
179 template <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 
189 template <typename PointIn, typename PointOut> inline PointOut
190 Convolution<PointIn, PointOut>::convolveOneRowNonDense (int i, int j)
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 
215 template <typename PointIn, typename PointOut> inline PointOut
216 Convolution<PointIn, PointOut>::convolveOneColNonDense (int i, int j)
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 
241 template<> pcl::PointXYZRGB
242 PCL_EXPORTS Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowDense (int i, int j);
243 
244 template<> pcl::PointXYZRGB
245 PCL_EXPORTS Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColDense (int i, int j);
246 
247 template<> pcl::PointXYZRGB
248 PCL_EXPORTS Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowNonDense (int i, int j);
249 
250 template<> pcl::PointXYZRGB
251 PCL_EXPORTS Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColNonDense (int i, int j);
252 
253 template<> pcl::RGB
254 PCL_EXPORTS Convolution<pcl::RGB, pcl::RGB>::convolveOneRowDense (int i, int j);
255 
256 template<> pcl::RGB
257 PCL_EXPORTS Convolution<pcl::RGB, pcl::RGB>::convolveOneColDense (int i, int j);
258 
259 template<> inline pcl::RGB
260 Convolution<pcl::RGB, pcl::RGB>::convolveOneRowNonDense (int i, int j)
261 {
262  return (convolveOneRowDense (i,j));
263 }
264 
265 template<> inline pcl::RGB
266 Convolution<pcl::RGB, pcl::RGB>::convolveOneColNonDense (int i, int j)
267 {
268  return (convolveOneColDense (i,j));
269 }
270 
271 template<> inline void
273 {
274  p.r = 0; p.g = 0; p.b = 0;
275 }
276 
277 template <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 
323 template <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 
370 template <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 
417 template <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 
463 template <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 
510 template <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
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
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:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
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)
Definition: convolution.h:224
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.
Definition: convolution.hpp:92
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...
void initCompute(PointCloudOut &output)
init compute is an internal method called before computation
Definition: convolution.hpp:61
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
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A point structure representing Euclidean xyz coordinates, and the RGB color.
A structure representing RGB color information.