Point Cloud Library (PCL)  1.9.0-dev
pyramid.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, 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_IMPL_PYRAMID_HPP
41 #define PCL_FILTERS_IMPL_PYRAMID_HPP
42 
43 template <typename PointT> bool
45 {
46  if (!input_->isOrganized ())
47  {
48  PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ());
49  return (false);
50  }
51 
52  if (levels_ < 2)
53  {
54  PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ());
55  return (false);
56  }
57 
58  // size_t ratio (std::pow (2, levels_));
59  // size_t last_width = input_->width / ratio;
60  // size_t last_height = input_->height / ratio;
61 
62  if (levels_ > 4)
63  {
64  PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!", getClassName ().c_str ());
65  return (false);
66  }
67 
68  if (large_)
69  {
70  Eigen::VectorXf k (5);
71  k << 1.f/16.f, 1.f/4.f, 3.f/8.f, 1.f/4.f, 1.f/16.f;
72  kernel_ = k * k.transpose ();
73  if (threshold_ != std::numeric_limits<float>::infinity ())
74  threshold_ *= 2 * threshold_;
75 
76  }
77  else
78  {
79  Eigen::VectorXf k (3);
80  k << 1.f/4.f, 1.f/2.f, 1.f/4.f;
81  kernel_ = k * k.transpose ();
82  if (threshold_ != std::numeric_limits<float>::infinity ())
83  threshold_ *= threshold_;
84  }
85 
86  return (true);
87 }
88 
89 template <typename PointT> void
90 pcl::filters::Pyramid<PointT>::compute (std::vector<PointCloudPtr>& output)
91 {
92  std::cout << "compute" << std::endl;
93  if (!initCompute ())
94  {
95  PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
96  return;
97  }
98 
99  int kernel_rows = static_cast<int> (kernel_.rows ());
100  int kernel_cols = static_cast<int> (kernel_.cols ());
101  int kernel_center_x = kernel_cols / 2;
102  int kernel_center_y = kernel_rows / 2;
103 
104  output.resize (levels_ + 1);
105  output[0].reset (new pcl::PointCloud<PointT>);
106  *(output[0]) = *input_;
107 
108  if (input_->is_dense)
109  {
110  for (int l = 1; l <= levels_; ++l)
111  {
112  output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
113  const PointCloud<PointT> &previous = *output[l-1];
114  PointCloud<PointT> &next = *output[l];
115 #ifdef _OPENMP
116 #pragma omp parallel for shared (next) num_threads(threads_)
117 #endif
118  for(int i=0; i < next.height; ++i)
119  {
120  for(int j=0; j < next.width; ++j)
121  {
122  for(int m=0; m < kernel_rows; ++m)
123  {
124  int mm = kernel_rows - 1 - m;
125  for(int n=0; n < kernel_cols; ++n)
126  {
127  int nn = kernel_cols - 1 - n;
128 
129  int ii = 2*i + (m - kernel_center_y);
130  int jj = 2*j + (n - kernel_center_x);
131 
132  if (ii < 0) ii = 0;
133  if (ii >= previous.height) ii = previous.height - 1;
134  if (jj < 0) jj = 0;
135  if (jj >= previous.width) jj = previous.width - 1;
136  next.at (j,i) += previous.at (jj,ii) * kernel_ (mm,nn);
137  }
138  }
139  }
140  }
141  }
142  }
143  else
144  {
145  for (int l = 1; l <= levels_; ++l)
146  {
147  output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
148  const PointCloud<PointT> &previous = *output[l-1];
149  PointCloud<PointT> &next = *output[l];
150 #ifdef _OPENMP
151 #pragma omp parallel for shared (next) num_threads(threads_)
152 #endif
153  for(int i=0; i < next.height; ++i)
154  {
155  for(int j=0; j < next.width; ++j)
156  {
157  float weight = 0;
158  for(int m=0; m < kernel_rows; ++m)
159  {
160  int mm = kernel_rows - 1 - m;
161  for(int n=0; n < kernel_cols; ++n)
162  {
163  int nn = kernel_cols - 1 - n;
164  int ii = 2*i + (m - kernel_center_y);
165  int jj = 2*j + (n - kernel_center_x);
166  if (ii < 0) ii = 0;
167  if (ii >= previous.height) ii = previous.height - 1;
168  if (jj < 0) jj = 0;
169  if (jj >= previous.width) jj = previous.width - 1;
170  if (!isFinite (previous.at (jj,ii)))
171  continue;
172  if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
173  {
174  next.at (j,i) += previous.at (jj,ii).x * kernel_ (mm,nn);
175  weight+= kernel_ (mm,nn);
176  }
177  }
178  }
179  if (weight == 0)
180  nullify (next.at (j,i));
181  else
182  {
183  weight = 1.f/weight;
184  next.at (j,i)*= weight;
185  }
186  }
187  }
188  }
189  }
190 }
191 
192 namespace pcl
193 {
194  namespace filters
195  {
196  template <> void
198  {
199  std::cout << "PointXYZRGB" << std::endl;
200  if (!initCompute ())
201  {
202  PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
203  return;
204  }
205 
206  int kernel_rows = static_cast<int> (kernel_.rows ());
207  int kernel_cols = static_cast<int> (kernel_.cols ());
208  int kernel_center_x = kernel_cols / 2;
209  int kernel_center_y = kernel_rows / 2;
210 
211  output.resize (levels_ + 1);
212  output[0].reset (new pcl::PointCloud<pcl::PointXYZRGB>);
213  *(output[0]) = *input_;
214 
215  if (input_->is_dense)
216  {
217  for (int l = 1; l <= levels_; ++l)
218  {
219  output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
220  const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
221  PointCloud<pcl::PointXYZRGB> &next = *output[l];
222 #ifdef _OPENMP
223 #pragma omp parallel for shared (next) num_threads(threads_)
224 #endif
225  for(int i=0; i < next.height; ++i) // rows
226  {
227  for(int j=0; j < next.width; ++j) // columns
228  {
229  float r = 0, g = 0, b = 0;
230  for(int m=0; m < kernel_rows; ++m) // kernel rows
231  {
232  int mm = kernel_rows - 1 - m; // row index of flipped kernel
233  for(int n=0; n < kernel_cols; ++n) // kernel columns
234  {
235  int nn = kernel_cols - 1 - n; // column index of flipped kernel
236  // index of input signal, used for checking boundary
237  int ii = 2*i + (m - kernel_center_y);
238  int jj = 2*j + (n - kernel_center_x);
239 
240  // ignore input samples which are out of bound
241  if (ii < 0) ii = 0;
242  if (ii >= previous.height) ii = previous.height - 1;
243  if (jj < 0) jj = 0;
244  if (jj >= previous.width) jj = previous.width - 1;
245  next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
246  next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
247  next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
248  b += previous.at (jj,ii).b * kernel_ (mm,nn);
249  g += previous.at (jj,ii).g * kernel_ (mm,nn);
250  r += previous.at (jj,ii).r * kernel_ (mm,nn);
251  }
252  }
253  next.at (j,i).b = static_cast<pcl::uint8_t> (b);
254  next.at (j,i).g = static_cast<pcl::uint8_t> (g);
255  next.at (j,i).r = static_cast<pcl::uint8_t> (r);
256  }
257  }
258  }
259  }
260  else
261  {
262  for (int l = 1; l <= levels_; ++l)
263  {
264  output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
265  const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
266  PointCloud<pcl::PointXYZRGB> &next = *output[l];
267 #ifdef _OPENMP
268 #pragma omp parallel for shared (next) num_threads(threads_)
269 #endif
270  for(int i=0; i < next.height; ++i)
271  {
272  for(int j=0; j < next.width; ++j)
273  {
274  float weight = 0;
275  float r = 0, g = 0, b = 0;
276  for(int m=0; m < kernel_rows; ++m)
277  {
278  int mm = kernel_rows - 1 - m;
279  for(int n=0; n < kernel_cols; ++n)
280  {
281  int nn = kernel_cols - 1 - n;
282  int ii = 2*i + (m - kernel_center_y);
283  int jj = 2*j + (n - kernel_center_x);
284  if (ii < 0) ii = 0;
285  if (ii >= previous.height) ii = previous.height - 1;
286  if (jj < 0) jj = 0;
287  if (jj >= previous.width) jj = previous.width - 1;
288  if (!isFinite (previous.at (jj,ii)))
289  continue;
290  if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
291  {
292  next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
293  next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
294  next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
295  b += previous.at (jj,ii).b * kernel_ (mm,nn);
296  g += previous.at (jj,ii).g * kernel_ (mm,nn);
297  r += previous.at (jj,ii).r * kernel_ (mm,nn);
298  weight+= kernel_ (mm,nn);
299  }
300  }
301  }
302  if (weight == 0)
303  nullify (next.at (j,i));
304  else
305  {
306  weight = 1.f/weight;
307  r*= weight; g*= weight; b*= weight;
308  next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight;
309  next.at (j,i).b = static_cast<pcl::uint8_t> (b);
310  next.at (j,i).g = static_cast<pcl::uint8_t> (g);
311  next.at (j,i).r = static_cast<pcl::uint8_t> (r);
312  }
313  }
314  }
315  }
316  }
317  }
318 
319  template <> void
321  {
322  std::cout << "PointXYZRGBA" << std::endl;
323  if (!initCompute ())
324  {
325  PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
326  return;
327  }
328 
329  int kernel_rows = static_cast<int> (kernel_.rows ());
330  int kernel_cols = static_cast<int> (kernel_.cols ());
331  int kernel_center_x = kernel_cols / 2;
332  int kernel_center_y = kernel_rows / 2;
333 
334  output.resize (levels_ + 1);
335  output[0].reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
336  *(output[0]) = *input_;
337 
338  if (input_->is_dense)
339  {
340  for (int l = 1; l <= levels_; ++l)
341  {
342  output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
343  const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
344  PointCloud<pcl::PointXYZRGBA> &next = *output[l];
345 #ifdef _OPENMP
346 #pragma omp parallel for shared (next) num_threads(threads_)
347 #endif
348  for(int i=0; i < next.height; ++i) // rows
349  {
350  for(int j=0; j < next.width; ++j) // columns
351  {
352  float r = 0, g = 0, b = 0, a = 0;
353  for(int m=0; m < kernel_rows; ++m) // kernel rows
354  {
355  int mm = kernel_rows - 1 - m; // row index of flipped kernel
356  for(int n=0; n < kernel_cols; ++n) // kernel columns
357  {
358  int nn = kernel_cols - 1 - n; // column index of flipped kernel
359  // index of input signal, used for checking boundary
360  int ii = 2*i + (m - kernel_center_y);
361  int jj = 2*j + (n - kernel_center_x);
362 
363  // ignore input samples which are out of bound
364  if (ii < 0) ii = 0;
365  if (ii >= previous.height) ii = previous.height - 1;
366  if (jj < 0) jj = 0;
367  if (jj >= previous.width) jj = previous.width - 1;
368  next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
369  next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
370  next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
371  b += previous.at (jj,ii).b * kernel_ (mm,nn);
372  g += previous.at (jj,ii).g * kernel_ (mm,nn);
373  r += previous.at (jj,ii).r * kernel_ (mm,nn);
374  a += previous.at (jj,ii).a * kernel_ (mm,nn);
375  }
376  }
377  next.at (j,i).b = static_cast<pcl::uint8_t> (b);
378  next.at (j,i).g = static_cast<pcl::uint8_t> (g);
379  next.at (j,i).r = static_cast<pcl::uint8_t> (r);
380  next.at (j,i).a = static_cast<pcl::uint8_t> (a);
381  }
382  }
383  }
384  }
385  else
386  {
387  for (int l = 1; l <= levels_; ++l)
388  {
389  output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
390  const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
391  PointCloud<pcl::PointXYZRGBA> &next = *output[l];
392 #ifdef _OPENMP
393 #pragma omp parallel for shared (next) num_threads(threads_)
394 #endif
395  for(int i=0; i < next.height; ++i)
396  {
397  for(int j=0; j < next.width; ++j)
398  {
399  float weight = 0;
400  float r = 0, g = 0, b = 0, a = 0;
401  for(int m=0; m < kernel_rows; ++m)
402  {
403  int mm = kernel_rows - 1 - m;
404  for(int n=0; n < kernel_cols; ++n)
405  {
406  int nn = kernel_cols - 1 - n;
407  int ii = 2*i + (m - kernel_center_y);
408  int jj = 2*j + (n - kernel_center_x);
409  if (ii < 0) ii = 0;
410  if (ii >= previous.height) ii = previous.height - 1;
411  if (jj < 0) jj = 0;
412  if (jj >= previous.width) jj = previous.width - 1;
413  if (!isFinite (previous.at (jj,ii)))
414  continue;
415  if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
416  {
417  next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn);
418  next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn);
419  next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn);
420  b += previous.at (jj,ii).b * kernel_ (mm,nn);
421  g += previous.at (jj,ii).g * kernel_ (mm,nn);
422  r += previous.at (jj,ii).r * kernel_ (mm,nn);
423  a += previous.at (jj,ii).a * kernel_ (mm,nn);
424  weight+= kernel_ (mm,nn);
425  }
426  }
427  }
428  if (weight == 0)
429  nullify (next.at (j,i));
430  else
431  {
432  weight = 1.f/weight;
433  r*= weight; g*= weight; b*= weight; a*= weight;
434  next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight;
435  next.at (j,i).b = static_cast<pcl::uint8_t> (b);
436  next.at (j,i).g = static_cast<pcl::uint8_t> (g);
437  next.at (j,i).r = static_cast<pcl::uint8_t> (r);
438  next.at (j,i).a = static_cast<pcl::uint8_t> (a);
439  }
440  }
441  }
442  }
443  }
444  }
445 
446  template<> void
448  {
449  p.r = 0; p.g = 0; p.b = 0;
450  }
451 
452  template <> void
454  {
455  std::cout << "RGB" << std::endl;
456  if (!initCompute ())
457  {
458  PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
459  return;
460  }
461 
462  int kernel_rows = static_cast<int> (kernel_.rows ());
463  int kernel_cols = static_cast<int> (kernel_.cols ());
464  int kernel_center_x = kernel_cols / 2;
465  int kernel_center_y = kernel_rows / 2;
466 
467  output.resize (levels_ + 1);
468  output[0].reset (new pcl::PointCloud<pcl::RGB>);
469  *(output[0]) = *input_;
470 
471  if (input_->is_dense)
472  {
473  for (int l = 1; l <= levels_; ++l)
474  {
475  output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
476  const PointCloud<pcl::RGB> &previous = *output[l-1];
477  PointCloud<pcl::RGB> &next = *output[l];
478 #ifdef _OPENMP
479 #pragma omp parallel for shared (next) num_threads(threads_)
480 #endif
481  for(int i=0; i < next.height; ++i)
482  {
483  for(int j=0; j < next.width; ++j)
484  {
485  float r = 0, g = 0, b = 0;
486  for(int m=0; m < kernel_rows; ++m)
487  {
488  int mm = kernel_rows - 1 - m;
489  for(int n=0; n < kernel_cols; ++n)
490  {
491  int nn = kernel_cols - 1 - n;
492  int ii = 2*i + (m - kernel_center_y);
493  int jj = 2*j + (n - kernel_center_x);
494  if (ii < 0) ii = 0;
495  if (ii >= previous.height) ii = previous.height - 1;
496  if (jj < 0) jj = 0;
497  if (jj >= previous.width) jj = previous.width - 1;
498  b += previous.at (jj,ii).b * kernel_ (mm,nn);
499  g += previous.at (jj,ii).g * kernel_ (mm,nn);
500  r += previous.at (jj,ii).r * kernel_ (mm,nn);
501  }
502  }
503  next.at (j,i).b = static_cast<pcl::uint8_t> (b);
504  next.at (j,i).g = static_cast<pcl::uint8_t> (g);
505  next.at (j,i).r = static_cast<pcl::uint8_t> (r);
506  }
507  }
508  }
509  }
510  else
511  {
512  for (int l = 1; l <= levels_; ++l)
513  {
514  output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
515  const PointCloud<pcl::RGB> &previous = *output[l-1];
516  PointCloud<pcl::RGB> &next = *output[l];
517 #ifdef _OPENMP
518 #pragma omp parallel for shared (next) num_threads(threads_)
519 #endif
520  for(int i=0; i < next.height; ++i)
521  {
522  for(int j=0; j < next.width; ++j)
523  {
524  float weight = 0;
525  float r = 0, g = 0, b = 0;
526  for(int m=0; m < kernel_rows; ++m)
527  {
528  int mm = kernel_rows - 1 - m;
529  for(int n=0; n < kernel_cols; ++n)
530  {
531  int nn = kernel_cols - 1 - n;
532  int ii = 2*i + (m - kernel_center_y);
533  int jj = 2*j + (n - kernel_center_x);
534  if (ii < 0) ii = 0;
535  if (ii >= previous.height) ii = previous.height - 1;
536  if (jj < 0) jj = 0;
537  if (jj >= previous.width) jj = previous.width - 1;
538  if (!isFinite (previous.at (jj,ii)))
539  continue;
540  if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
541  {
542  b += previous.at (jj,ii).b * kernel_ (mm,nn);
543  g += previous.at (jj,ii).g * kernel_ (mm,nn);
544  r += previous.at (jj,ii).r * kernel_ (mm,nn);
545  weight+= kernel_ (mm,nn);
546  }
547  }
548  }
549  if (weight == 0)
550  nullify (next.at (j,i));
551  else
552  {
553  weight = 1.f/weight;
554  r*= weight; g*= weight; b*= weight;
555  next.at (j,i).b = static_cast<pcl::uint8_t> (b);
556  next.at (j,i).g = static_cast<pcl::uint8_t> (g);
557  next.at (j,i).r = static_cast<pcl::uint8_t> (r);
558  }
559  }
560  }
561  }
562  }
563  }
564 
565  }
566 }
567 
568 #endif
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:54
PointCloud< PointT >::Ptr PointCloudPtr
Definition: pyramid.h:65
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
Pyramid constructs a multi-scale representation of an organised point cloud.
Definition: pyramid.h:62
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:174
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
void compute(std::vector< PointCloudPtr > &output)
compute the pyramid levels.
Definition: pyramid.hpp:90
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
PointCloud represents the base class in PCL for storing collections of 3D points. ...
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:283