41 #ifndef PCL_FILTERS_IMPL_PYRAMID_HPP
42 #define PCL_FILTERS_IMPL_PYRAMID_HPP
51 template <
typename Po
intT>
bool
52 Pyramid<PointT>::initCompute ()
54 if (!input_->isOrganized ())
56 PCL_ERROR (
"[pcl::fileters::%s::initCompute] Number of levels should be at least 2!\n", getClassName ().c_str ());
62 PCL_ERROR (
"[pcl::fileters::%s::initCompute] Number of levels should be at least 2!\n", getClassName ().c_str ());
72 PCL_ERROR (
"[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!\n", getClassName ().c_str ());
78 Eigen::VectorXf k (5);
79 k << 1.f/16.f, 1.f/4.f, 3.f/8.f, 1.f/4.f, 1.f/16.f;
80 kernel_ = k * k.transpose ();
81 if (threshold_ != std::numeric_limits<float>::infinity ())
82 threshold_ *= 2 * threshold_;
87 Eigen::VectorXf k (3);
88 k << 1.f/4.f, 1.f/2.f, 1.f/4.f;
89 kernel_ = k * k.transpose ();
90 if (threshold_ != std::numeric_limits<float>::infinity ())
91 threshold_ *= threshold_;
97 template <
typename Po
intT>
void
100 std::cout <<
"compute" << std::endl;
103 PCL_ERROR (
"[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
107 int kernel_rows =
static_cast<int> (kernel_.rows ());
108 int kernel_cols =
static_cast<int> (kernel_.cols ());
109 int kernel_center_x = kernel_cols / 2;
110 int kernel_center_y = kernel_rows / 2;
112 output.resize (levels_ + 1);
114 *(output[0]) = *input_;
116 if (input_->is_dense)
118 for (
int l = 1; l <= levels_; ++l)
123 #pragma omp parallel for \
126 num_threads(threads_)
127 for(
int i=0; i < next.
height; ++i)
129 for(
int j=0; j < next.
width; ++j)
131 for(
int m=0; m < kernel_rows; ++m)
133 int mm = kernel_rows - 1 - m;
134 for(
int n=0; n < kernel_cols; ++n)
136 int nn = kernel_cols - 1 - n;
138 int ii = 2*i + (m - kernel_center_y);
139 int jj = 2*j + (n - kernel_center_x);
144 if (jj >= previous.
width) jj = previous.
width - 1;
145 next.
at (j,i) += previous.
at (jj,ii) * kernel_ (mm,nn);
154 for (
int l = 1; l <= levels_; ++l)
159 #pragma omp parallel for \
162 num_threads(threads_)
163 for(
int i=0; i < next.
height; ++i)
165 for(
int j=0; j < next.
width; ++j)
168 for(
int m=0; m < kernel_rows; ++m)
170 int mm = kernel_rows - 1 - m;
171 for(
int n=0; n < kernel_cols; ++n)
173 int nn = kernel_cols - 1 - n;
174 int ii = 2*i + (m - kernel_center_y);
175 int jj = 2*j + (n - kernel_center_x);
179 if (jj >= previous.
width) jj = previous.
width - 1;
184 next.
at (j,i) += previous.
at (jj,ii).x * kernel_ (mm,nn);
185 weight+= kernel_ (mm,nn);
190 nullify (next.
at (j,i));
194 next.
at (j,i)*= weight;
206 std::cout <<
"PointXYZRGB" << std::endl;
209 PCL_ERROR (
"[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
213 int kernel_rows =
static_cast<int> (kernel_.rows ());
214 int kernel_cols =
static_cast<int> (kernel_.cols ());
215 int kernel_center_x = kernel_cols / 2;
216 int kernel_center_y = kernel_rows / 2;
218 output.resize (levels_ + 1);
220 *(output[0]) = *input_;
222 if (input_->is_dense)
224 for (
int l = 1; l <= levels_; ++l)
229 #pragma omp parallel for \
232 num_threads(threads_)
233 for(
int i=0; i < next.
height; ++i)
235 for(
int j=0; j < next.
width; ++j)
237 float r = 0, g = 0, b = 0;
238 for(
int m=0; m < kernel_rows; ++m)
240 int mm = kernel_rows - 1 - m;
241 for(
int n=0; n < kernel_cols; ++n)
243 int nn = kernel_cols - 1 - n;
245 int ii = 2*i + (m - kernel_center_y);
246 int jj = 2*j + (n - kernel_center_x);
252 if (jj >= previous.
width) jj = previous.
width - 1;
253 next.
at (j,i).x += previous.
at (jj,ii).x * kernel_ (mm,nn);
254 next.
at (j,i).y += previous.
at (jj,ii).y * kernel_ (mm,nn);
255 next.
at (j,i).z += previous.
at (jj,ii).z * kernel_ (mm,nn);
256 b += previous.
at (jj,ii).b * kernel_ (mm,nn);
257 g += previous.
at (jj,ii).g * kernel_ (mm,nn);
258 r += previous.
at (jj,ii).r * kernel_ (mm,nn);
261 next.
at (j,i).b =
static_cast<std::uint8_t
> (b);
262 next.
at (j,i).g =
static_cast<std::uint8_t
> (g);
263 next.
at (j,i).r =
static_cast<std::uint8_t
> (r);
270 for (
int l = 1; l <= levels_; ++l)
275 #pragma omp parallel for \
278 num_threads(threads_)
279 for(
int i=0; i < next.
height; ++i)
281 for(
int j=0; j < next.
width; ++j)
284 float r = 0, g = 0, b = 0;
285 for(
int m=0; m < kernel_rows; ++m)
287 int mm = kernel_rows - 1 - m;
288 for(
int n=0; n < kernel_cols; ++n)
290 int nn = kernel_cols - 1 - n;
291 int ii = 2*i + (m - kernel_center_y);
292 int jj = 2*j + (n - kernel_center_x);
296 if (jj >= previous.
width) jj = previous.
width - 1;
301 next.
at (j,i).x += previous.
at (jj,ii).x * kernel_ (mm,nn);
302 next.
at (j,i).y += previous.
at (jj,ii).y * kernel_ (mm,nn);
303 next.
at (j,i).z += previous.
at (jj,ii).z * kernel_ (mm,nn);
304 b += previous.
at (jj,ii).b * kernel_ (mm,nn);
305 g += previous.
at (jj,ii).g * kernel_ (mm,nn);
306 r += previous.
at (jj,ii).r * kernel_ (mm,nn);
307 weight+= kernel_ (mm,nn);
312 nullify (next.
at (j,i));
316 r*= weight; g*= weight; b*= weight;
317 next.
at (j,i).x*= weight; next.
at (j,i).y*= weight; next.
at (j,i).z*= weight;
318 next.
at (j,i).b =
static_cast<std::uint8_t
> (b);
319 next.
at (j,i).g =
static_cast<std::uint8_t
> (g);
320 next.
at (j,i).r =
static_cast<std::uint8_t
> (r);
331 std::cout <<
"PointXYZRGBA" << std::endl;
334 PCL_ERROR (
"[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
338 int kernel_rows =
static_cast<int> (kernel_.rows ());
339 int kernel_cols =
static_cast<int> (kernel_.cols ());
340 int kernel_center_x = kernel_cols / 2;
341 int kernel_center_y = kernel_rows / 2;
343 output.resize (levels_ + 1);
345 *(output[0]) = *input_;
347 if (input_->is_dense)
349 for (
int l = 1; l <= levels_; ++l)
354 #pragma omp parallel for \
357 num_threads(threads_)
358 for(
int i=0; i < next.
height; ++i)
360 for(
int j=0; j < next.
width; ++j)
362 float r = 0, g = 0, b = 0, a = 0;
363 for(
int m=0; m < kernel_rows; ++m)
365 int mm = kernel_rows - 1 - m;
366 for(
int n=0; n < kernel_cols; ++n)
368 int nn = kernel_cols - 1 - n;
370 int ii = 2*i + (m - kernel_center_y);
371 int jj = 2*j + (n - kernel_center_x);
377 if (jj >= previous.
width) jj = previous.
width - 1;
378 next.
at (j,i).x += previous.
at (jj,ii).x * kernel_ (mm,nn);
379 next.
at (j,i).y += previous.
at (jj,ii).y * kernel_ (mm,nn);
380 next.
at (j,i).z += previous.
at (jj,ii).z * kernel_ (mm,nn);
381 b += previous.
at (jj,ii).b * kernel_ (mm,nn);
382 g += previous.
at (jj,ii).g * kernel_ (mm,nn);
383 r += previous.
at (jj,ii).r * kernel_ (mm,nn);
384 a += previous.
at (jj,ii).a * kernel_ (mm,nn);
387 next.
at (j,i).b =
static_cast<std::uint8_t
> (b);
388 next.
at (j,i).g =
static_cast<std::uint8_t
> (g);
389 next.
at (j,i).r =
static_cast<std::uint8_t
> (r);
390 next.
at (j,i).a =
static_cast<std::uint8_t
> (a);
397 for (
int l = 1; l <= levels_; ++l)
402 #pragma omp parallel for \
405 num_threads(threads_)
406 for(
int i=0; i < next.
height; ++i)
408 for(
int j=0; j < next.
width; ++j)
411 float r = 0, g = 0, b = 0, a = 0;
412 for(
int m=0; m < kernel_rows; ++m)
414 int mm = kernel_rows - 1 - m;
415 for(
int n=0; n < kernel_cols; ++n)
417 int nn = kernel_cols - 1 - n;
418 int ii = 2*i + (m - kernel_center_y);
419 int jj = 2*j + (n - kernel_center_x);
423 if (jj >= previous.
width) jj = previous.
width - 1;
428 next.
at (j,i).x += previous.
at (jj,ii).x * kernel_ (mm,nn);
429 next.
at (j,i).y += previous.
at (jj,ii).y * kernel_ (mm,nn);
430 next.
at (j,i).z += previous.
at (jj,ii).z * kernel_ (mm,nn);
431 b += previous.
at (jj,ii).b * kernel_ (mm,nn);
432 g += previous.
at (jj,ii).g * kernel_ (mm,nn);
433 r += previous.
at (jj,ii).r * kernel_ (mm,nn);
434 a += previous.
at (jj,ii).a * kernel_ (mm,nn);
435 weight+= kernel_ (mm,nn);
440 nullify (next.
at (j,i));
444 r*= weight; g*= weight; b*= weight; a*= weight;
445 next.
at (j,i).x*= weight; next.
at (j,i).y*= weight; next.
at (j,i).z*= weight;
446 next.
at (j,i).b =
static_cast<std::uint8_t
> (b);
447 next.
at (j,i).g =
static_cast<std::uint8_t
> (g);
448 next.
at (j,i).r =
static_cast<std::uint8_t
> (r);
449 next.
at (j,i).a =
static_cast<std::uint8_t
> (a);
460 p.r = 0; p.g = 0; p.b = 0;
466 std::cout <<
"RGB" << std::endl;
469 PCL_ERROR (
"[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
473 int kernel_rows =
static_cast<int> (kernel_.rows ());
474 int kernel_cols =
static_cast<int> (kernel_.cols ());
475 int kernel_center_x = kernel_cols / 2;
476 int kernel_center_y = kernel_rows / 2;
478 output.resize (levels_ + 1);
480 *(output[0]) = *input_;
482 if (input_->is_dense)
484 for (
int l = 1; l <= levels_; ++l)
489 #pragma omp parallel for \
492 num_threads(threads_)
493 for(
int i=0; i < next.
height; ++i)
495 for(
int j=0; j < next.
width; ++j)
497 float r = 0, g = 0, b = 0;
498 for(
int m=0; m < kernel_rows; ++m)
500 int mm = kernel_rows - 1 - m;
501 for(
int n=0; n < kernel_cols; ++n)
503 int nn = kernel_cols - 1 - n;
504 int ii = 2*i + (m - kernel_center_y);
505 int jj = 2*j + (n - kernel_center_x);
509 if (jj >= previous.
width) jj = previous.
width - 1;
510 b += previous.
at (jj,ii).b * kernel_ (mm,nn);
511 g += previous.
at (jj,ii).g * kernel_ (mm,nn);
512 r += previous.
at (jj,ii).r * kernel_ (mm,nn);
515 next.
at (j,i).b =
static_cast<std::uint8_t
> (b);
516 next.
at (j,i).g =
static_cast<std::uint8_t
> (g);
517 next.
at (j,i).r =
static_cast<std::uint8_t
> (r);
524 for (
int l = 1; l <= levels_; ++l)
529 #pragma omp parallel for \
532 num_threads(threads_)
533 for(
int i=0; i < next.
height; ++i)
535 for(
int j=0; j < next.
width; ++j)
538 float r = 0, g = 0, b = 0;
539 for(
int m=0; m < kernel_rows; ++m)
541 int mm = kernel_rows - 1 - m;
542 for(
int n=0; n < kernel_cols; ++n)
544 int nn = kernel_cols - 1 - n;
545 int ii = 2*i + (m - kernel_center_y);
546 int jj = 2*j + (n - kernel_center_x);
550 if (jj >= previous.
width) jj = previous.
width - 1;
555 b += previous.
at (jj,ii).b * kernel_ (mm,nn);
556 g += previous.
at (jj,ii).g * kernel_ (mm,nn);
557 r += previous.
at (jj,ii).r * kernel_ (mm,nn);
558 weight+= kernel_ (mm,nn);
563 nullify (next.
at (j,i));
567 r*= weight; g*= weight; b*= weight;
568 next.
at (j,i).b =
static_cast<std::uint8_t
> (b);
569 next.
at (j,i).g =
static_cast<std::uint8_t
> (g);
570 next.
at (j,i).r =
static_cast<std::uint8_t
> (r);