Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
41#ifndef PCL_FILTERS_IMPL_PYRAMID_HPP
42#define PCL_FILTERS_IMPL_PYRAMID_HPP
43
44
45namespace pcl
46{
47
48namespace filters
49{
50
51template <typename PointT> bool
52Pyramid<PointT>::initCompute ()
53{
54 if (!input_->isOrganized ())
55 {
56 PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!\n", getClassName ().c_str ());
57 return (false);
58 }
59
60 if (levels_ < 2)
61 {
62 PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!\n", getClassName ().c_str ());
63 return (false);
64 }
65
66 // std::size_t ratio (std::pow (2, levels_));
67 // std::size_t last_width = input_->width / ratio;
68 // std::size_t last_height = input_->height / ratio;
69
70 if (levels_ > 4)
71 {
72 PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!\n", getClassName ().c_str ());
73 return (false);
74 }
75
76 if (large_)
77 {
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_;
83
84 }
85 else
86 {
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_;
92 }
93
94 return (true);
95}
96
97template <typename PointT> void
98Pyramid<PointT>::compute (std::vector<PointCloudPtr>& output)
99{
100 std::cout << "compute" << std::endl;
101 if (!initCompute ())
102 {
103 PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
104 return;
105 }
106
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;
111
112 output.resize (levels_ + 1);
113 output[0].reset (new pcl::PointCloud<PointT>);
114 *(output[0]) = *input_;
115
116 if (input_->is_dense)
117 {
118 for (int l = 1; l <= levels_; ++l)
119 {
120 output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
121 const PointCloud<PointT> &previous = *output[l-1];
122 PointCloud<PointT> &next = *output[l];
123#pragma omp parallel for \
124 default(none) \
125 shared(next) \
126 num_threads(threads_)
127 for(int i=0; i < next.height; ++i)
128 {
129 for(int j=0; j < next.width; ++j)
130 {
131 for(int m=0; m < kernel_rows; ++m)
132 {
133 int mm = kernel_rows - 1 - m;
134 for(int n=0; n < kernel_cols; ++n)
135 {
136 int nn = kernel_cols - 1 - n;
137
138 int ii = 2*i + (m - kernel_center_y);
139 int jj = 2*j + (n - kernel_center_x);
140
141 if (ii < 0) ii = 0;
142 if (ii >= previous.height) ii = previous.height - 1;
143 if (jj < 0) jj = 0;
144 if (jj >= previous.width) jj = previous.width - 1;
145 next.at (j,i) += previous.at (jj,ii) * kernel_ (mm,nn);
146 }
147 }
148 }
149 }
150 }
151 }
152 else
153 {
154 for (int l = 1; l <= levels_; ++l)
155 {
156 output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
157 const PointCloud<PointT> &previous = *output[l-1];
158 PointCloud<PointT> &next = *output[l];
159#pragma omp parallel for \
160 default(none) \
161 shared(next) \
162 num_threads(threads_)
163 for(int i=0; i < next.height; ++i)
164 {
165 for(int j=0; j < next.width; ++j)
166 {
167 float weight = 0;
168 for(int m=0; m < kernel_rows; ++m)
169 {
170 int mm = kernel_rows - 1 - m;
171 for(int n=0; n < kernel_cols; ++n)
172 {
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);
176 if (ii < 0) ii = 0;
177 if (ii >= previous.height) ii = previous.height - 1;
178 if (jj < 0) jj = 0;
179 if (jj >= previous.width) jj = previous.width - 1;
180 if (!isFinite (previous.at (jj,ii)))
181 continue;
182 if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
183 {
184 next.at (j,i) += previous.at (jj,ii).x * kernel_ (mm,nn);
185 weight+= kernel_ (mm,nn);
186 }
187 }
188 }
189 if (weight == 0)
190 nullify (next.at (j,i));
191 else
192 {
193 weight = 1.f/weight;
194 next.at (j,i)*= weight;
195 }
196 }
197 }
198 }
199 }
200}
201
202
203template <> void
205{
206 std::cout << "PointXYZRGB" << std::endl;
207 if (!initCompute ())
208 {
209 PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
210 return;
211 }
212
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;
217
218 output.resize (levels_ + 1);
219 output[0].reset (new pcl::PointCloud<pcl::PointXYZRGB>);
220 *(output[0]) = *input_;
221
222 if (input_->is_dense)
223 {
224 for (int l = 1; l <= levels_; ++l)
225 {
226 output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
227 const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
228 PointCloud<pcl::PointXYZRGB> &next = *output[l];
229#pragma omp parallel for \
230 default(none) \
231 shared(next) \
232 num_threads(threads_)
233 for(int i=0; i < next.height; ++i) // rows
234 {
235 for(int j=0; j < next.width; ++j) // columns
236 {
237 float r = 0, g = 0, b = 0;
238 for(int m=0; m < kernel_rows; ++m) // kernel rows
239 {
240 int mm = kernel_rows - 1 - m; // row index of flipped kernel
241 for(int n=0; n < kernel_cols; ++n) // kernel columns
242 {
243 int nn = kernel_cols - 1 - n; // column index of flipped kernel
244 // index of input signal, used for checking boundary
245 int ii = 2*i + (m - kernel_center_y);
246 int jj = 2*j + (n - kernel_center_x);
247
248 // ignore input samples which are out of bound
249 if (ii < 0) ii = 0;
250 if (ii >= previous.height) ii = previous.height - 1;
251 if (jj < 0) jj = 0;
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);
259 }
260 }
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);
264 }
265 }
266 }
267 }
268 else
269 {
270 for (int l = 1; l <= levels_; ++l)
271 {
272 output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2));
273 const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1];
274 PointCloud<pcl::PointXYZRGB> &next = *output[l];
275#pragma omp parallel for \
276 default(none) \
277 shared(next) \
278 num_threads(threads_)
279 for(int i=0; i < next.height; ++i)
280 {
281 for(int j=0; j < next.width; ++j)
282 {
283 float weight = 0;
284 float r = 0, g = 0, b = 0;
285 for(int m=0; m < kernel_rows; ++m)
286 {
287 int mm = kernel_rows - 1 - m;
288 for(int n=0; n < kernel_cols; ++n)
289 {
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);
293 if (ii < 0) ii = 0;
294 if (ii >= previous.height) ii = previous.height - 1;
295 if (jj < 0) jj = 0;
296 if (jj >= previous.width) jj = previous.width - 1;
297 if (!isFinite (previous.at (jj,ii)))
298 continue;
299 if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
300 {
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);
308 }
309 }
310 }
311 if (weight == 0)
312 nullify (next.at (j,i));
313 else
314 {
315 weight = 1.f/weight;
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);
321 }
322 }
323 }
324 }
325 }
326}
327
328template <> void
330{
331 std::cout << "PointXYZRGBA" << std::endl;
332 if (!initCompute ())
333 {
334 PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
335 return;
336 }
337
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;
342
343 output.resize (levels_ + 1);
344 output[0].reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
345 *(output[0]) = *input_;
346
347 if (input_->is_dense)
348 {
349 for (int l = 1; l <= levels_; ++l)
350 {
351 output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
352 const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
353 PointCloud<pcl::PointXYZRGBA> &next = *output[l];
354#pragma omp parallel for \
355 default(none) \
356 shared(next) \
357 num_threads(threads_)
358 for(int i=0; i < next.height; ++i) // rows
359 {
360 for(int j=0; j < next.width; ++j) // columns
361 {
362 float r = 0, g = 0, b = 0, a = 0;
363 for(int m=0; m < kernel_rows; ++m) // kernel rows
364 {
365 int mm = kernel_rows - 1 - m; // row index of flipped kernel
366 for(int n=0; n < kernel_cols; ++n) // kernel columns
367 {
368 int nn = kernel_cols - 1 - n; // column index of flipped kernel
369 // index of input signal, used for checking boundary
370 int ii = 2*i + (m - kernel_center_y);
371 int jj = 2*j + (n - kernel_center_x);
372
373 // ignore input samples which are out of bound
374 if (ii < 0) ii = 0;
375 if (ii >= previous.height) ii = previous.height - 1;
376 if (jj < 0) jj = 0;
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);
385 }
386 }
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);
391 }
392 }
393 }
394 }
395 else
396 {
397 for (int l = 1; l <= levels_; ++l)
398 {
399 output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2));
400 const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1];
401 PointCloud<pcl::PointXYZRGBA> &next = *output[l];
402#pragma omp parallel for \
403 default(none) \
404 shared(next) \
405 num_threads(threads_)
406 for(int i=0; i < next.height; ++i)
407 {
408 for(int j=0; j < next.width; ++j)
409 {
410 float weight = 0;
411 float r = 0, g = 0, b = 0, a = 0;
412 for(int m=0; m < kernel_rows; ++m)
413 {
414 int mm = kernel_rows - 1 - m;
415 for(int n=0; n < kernel_cols; ++n)
416 {
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);
420 if (ii < 0) ii = 0;
421 if (ii >= previous.height) ii = previous.height - 1;
422 if (jj < 0) jj = 0;
423 if (jj >= previous.width) jj = previous.width - 1;
424 if (!isFinite (previous.at (jj,ii)))
425 continue;
426 if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
427 {
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);
436 }
437 }
438 }
439 if (weight == 0)
440 nullify (next.at (j,i));
441 else
442 {
443 weight = 1.f/weight;
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);
450 }
451 }
452 }
453 }
454 }
455}
456
457template<> void
459{
460 p.r = 0; p.g = 0; p.b = 0;
461}
462
463template <> void
465{
466 std::cout << "RGB" << std::endl;
467 if (!initCompute ())
468 {
469 PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
470 return;
471 }
472
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;
477
478 output.resize (levels_ + 1);
479 output[0].reset (new pcl::PointCloud<pcl::RGB>);
480 *(output[0]) = *input_;
481
482 if (input_->is_dense)
483 {
484 for (int l = 1; l <= levels_; ++l)
485 {
486 output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
487 const PointCloud<pcl::RGB> &previous = *output[l-1];
488 PointCloud<pcl::RGB> &next = *output[l];
489#pragma omp parallel for \
490 default(none) \
491 shared(next) \
492 num_threads(threads_)
493 for(int i=0; i < next.height; ++i)
494 {
495 for(int j=0; j < next.width; ++j)
496 {
497 float r = 0, g = 0, b = 0;
498 for(int m=0; m < kernel_rows; ++m)
499 {
500 int mm = kernel_rows - 1 - m;
501 for(int n=0; n < kernel_cols; ++n)
502 {
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);
506 if (ii < 0) ii = 0;
507 if (ii >= previous.height) ii = previous.height - 1;
508 if (jj < 0) jj = 0;
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);
513 }
514 }
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);
518 }
519 }
520 }
521 }
522 else
523 {
524 for (int l = 1; l <= levels_; ++l)
525 {
526 output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2));
527 const PointCloud<pcl::RGB> &previous = *output[l-1];
528 PointCloud<pcl::RGB> &next = *output[l];
529#pragma omp parallel for \
530 default(none) \
531 shared(next) \
532 num_threads(threads_)
533 for(int i=0; i < next.height; ++i)
534 {
535 for(int j=0; j < next.width; ++j)
536 {
537 float weight = 0;
538 float r = 0, g = 0, b = 0;
539 for(int m=0; m < kernel_rows; ++m)
540 {
541 int mm = kernel_rows - 1 - m;
542 for(int n=0; n < kernel_cols; ++n)
543 {
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);
547 if (ii < 0) ii = 0;
548 if (ii >= previous.height) ii = previous.height - 1;
549 if (jj < 0) jj = 0;
550 if (jj >= previous.width) jj = previous.width - 1;
551 if (!isFinite (previous.at (jj,ii)))
552 continue;
553 if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
554 {
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);
559 }
560 }
561 }
562 if (weight == 0)
563 nullify (next.at (j,i));
564 else
565 {
566 weight = 1.f/weight;
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);
571 }
572 }
573 }
574 }
575 }
576}
577
578} // namespace filters
579} // namespace pcl
580
581#endif
582
PointCloud represents the base class in PCL for storing collections of 3D points.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Pyramid constructs a multi-scale representation of an organised point cloud.
Definition pyramid.h:63
void compute(std::vector< PointCloudPtr > &output)
compute the pyramid levels.
Definition pyramid.hpp:98
typename PointCloud< PointT >::Ptr PointCloudPtr
Definition pyramid.h:65
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
A structure representing RGB color information.