40 #ifndef PCL_FILTERS_IMPL_PYRAMID_HPP 41 #define PCL_FILTERS_IMPL_PYRAMID_HPP 43 template <
typename Po
intT>
bool 46 if (!input_->isOrganized ())
48 PCL_ERROR (
"[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ());
54 PCL_ERROR (
"[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ());
64 PCL_ERROR (
"[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!", getClassName ().c_str ());
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_;
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_;
89 template <
typename Po
intT>
void 92 std::cout <<
"compute" << std::endl;
95 PCL_ERROR (
"[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
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;
104 output.resize (levels_ + 1);
106 *(output[0]) = *input_;
108 if (input_->is_dense)
110 for (
int l = 1; l <= levels_; ++l)
116 #pragma omp parallel for shared (next) num_threads(threads_) 118 for(
int i=0; i < next.
height; ++i)
120 for(
int j=0; j < next.
width; ++j)
122 for(
int m=0; m < kernel_rows; ++m)
124 int mm = kernel_rows - 1 - m;
125 for(
int n=0; n < kernel_cols; ++n)
127 int nn = kernel_cols - 1 - n;
129 int ii = 2*i + (m - kernel_center_y);
130 int jj = 2*j + (n - kernel_center_x);
135 if (jj >= previous.
width) jj = previous.
width - 1;
136 next.
at (j,i) += previous.
at (jj,ii) * kernel_ (mm,nn);
145 for (
int l = 1; l <= levels_; ++l)
151 #pragma omp parallel for shared (next) num_threads(threads_) 153 for(
int i=0; i < next.
height; ++i)
155 for(
int j=0; j < next.
width; ++j)
158 for(
int m=0; m < kernel_rows; ++m)
160 int mm = kernel_rows - 1 - m;
161 for(
int n=0; n < kernel_cols; ++n)
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);
169 if (jj >= previous.
width) jj = previous.
width - 1;
174 next.
at (j,i) += previous.
at (jj,ii).x * kernel_ (mm,nn);
175 weight+= kernel_ (mm,nn);
180 nullify (next.
at (j,i));
184 next.
at (j,i)*= weight;
199 std::cout <<
"PointXYZRGB" << std::endl;
202 PCL_ERROR (
"[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
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;
211 output.resize (levels_ + 1);
213 *(output[0]) = *input_;
215 if (input_->is_dense)
217 for (
int l = 1; l <= levels_; ++l)
223 #pragma omp parallel for shared (next) num_threads(threads_) 225 for(
int i=0; i < next.
height; ++i)
227 for(
int j=0; j < next.
width; ++j)
229 float r = 0, g = 0, b = 0;
230 for(
int m=0; m < kernel_rows; ++m)
232 int mm = kernel_rows - 1 - m;
233 for(
int n=0; n < kernel_cols; ++n)
235 int nn = kernel_cols - 1 - n;
237 int ii = 2*i + (m - kernel_center_y);
238 int jj = 2*j + (n - kernel_center_x);
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);
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);
262 for (
int l = 1; l <= levels_; ++l)
268 #pragma omp parallel for shared (next) num_threads(threads_) 270 for(
int i=0; i < next.
height; ++i)
272 for(
int j=0; j < next.
width; ++j)
275 float r = 0, g = 0, b = 0;
276 for(
int m=0; m < kernel_rows; ++m)
278 int mm = kernel_rows - 1 - m;
279 for(
int n=0; n < kernel_cols; ++n)
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);
287 if (jj >= previous.
width) jj = previous.
width - 1;
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);
303 nullify (next.
at (j,i));
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);
322 std::cout <<
"PointXYZRGBA" << std::endl;
325 PCL_ERROR (
"[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
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;
334 output.resize (levels_ + 1);
336 *(output[0]) = *input_;
338 if (input_->is_dense)
340 for (
int l = 1; l <= levels_; ++l)
346 #pragma omp parallel for shared (next) num_threads(threads_) 348 for(
int i=0; i < next.
height; ++i)
350 for(
int j=0; j < next.
width; ++j)
352 float r = 0, g = 0, b = 0, a = 0;
353 for(
int m=0; m < kernel_rows; ++m)
355 int mm = kernel_rows - 1 - m;
356 for(
int n=0; n < kernel_cols; ++n)
358 int nn = kernel_cols - 1 - n;
360 int ii = 2*i + (m - kernel_center_y);
361 int jj = 2*j + (n - kernel_center_x);
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);
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);
387 for (
int l = 1; l <= levels_; ++l)
393 #pragma omp parallel for shared (next) num_threads(threads_) 395 for(
int i=0; i < next.
height; ++i)
397 for(
int j=0; j < next.
width; ++j)
400 float r = 0, g = 0, b = 0, a = 0;
401 for(
int m=0; m < kernel_rows; ++m)
403 int mm = kernel_rows - 1 - m;
404 for(
int n=0; n < kernel_cols; ++n)
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);
412 if (jj >= previous.
width) jj = previous.
width - 1;
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);
429 nullify (next.
at (j,i));
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);
449 p.r = 0; p.g = 0; p.b = 0;
455 std::cout <<
"RGB" << std::endl;
458 PCL_ERROR (
"[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
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;
467 output.resize (levels_ + 1);
469 *(output[0]) = *input_;
471 if (input_->is_dense)
473 for (
int l = 1; l <= levels_; ++l)
479 #pragma omp parallel for shared (next) num_threads(threads_) 481 for(
int i=0; i < next.
height; ++i)
483 for(
int j=0; j < next.
width; ++j)
485 float r = 0, g = 0, b = 0;
486 for(
int m=0; m < kernel_rows; ++m)
488 int mm = kernel_rows - 1 - m;
489 for(
int n=0; n < kernel_cols; ++n)
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);
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);
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);
512 for (
int l = 1; l <= levels_; ++l)
518 #pragma omp parallel for shared (next) num_threads(threads_) 520 for(
int i=0; i < next.
height; ++i)
522 for(
int j=0; j < next.
width; ++j)
525 float r = 0, g = 0, b = 0;
526 for(
int m=0; m < kernel_rows; ++m)
528 int mm = kernel_rows - 1 - m;
529 for(
int n=0; n < kernel_cols; ++n)
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);
537 if (jj >= previous.
width) jj = previous.
width - 1;
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);
550 nullify (next.
at (j,i));
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);
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...
PointCloud< PointT >::Ptr PointCloudPtr
Pyramid constructs a multi-scale representation of an organised point cloud.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
uint32_t height
The point cloud height (if organized as an image-structure).
void compute(std::vector< PointCloudPtr > &output)
compute the pyramid levels.
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
PointCloud represents the base class in PCL for storing collections of 3D points.