Point Cloud Library (PCL) 1.13.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
45#include <pcl/filters/pyramid.h>
46#include <pcl/console/print.h>
47#include <pcl/point_types.h>
48
49namespace pcl
50{
51namespace filters
52{
53template <typename PointT> bool
54Pyramid<PointT>::initCompute ()
55{
56 if (!input_->isOrganized ())
57 {
58 PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!\n", getClassName ().c_str ());
59 return (false);
60 }
61
62 if (levels_ < 2)
63 {
64 PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!\n", getClassName ().c_str ());
65 return (false);
66 }
67
68 // std::size_t ratio (std::pow (2, levels_));
69 // std::size_t last_width = input_->width / ratio;
70 // std::size_t last_height = input_->height / ratio;
71
72 if (levels_ > 4)
73 {
74 PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!\n", getClassName ().c_str ());
75 return (false);
76 }
77
78 if (large_)
79 {
80 Eigen::VectorXf k (5);
81 k << 1.f/16.f, 1.f/4.f, 3.f/8.f, 1.f/4.f, 1.f/16.f;
82 kernel_ = k * k.transpose ();
83 if (threshold_ != std::numeric_limits<float>::infinity ())
84 threshold_ *= 2 * threshold_;
85
86 }
87 else
88 {
89 Eigen::VectorXf k (3);
90 k << 1.f/4.f, 1.f/2.f, 1.f/4.f;
91 kernel_ = k * k.transpose ();
92 if (threshold_ != std::numeric_limits<float>::infinity ())
93 threshold_ *= threshold_;
94 }
95
96 return (true);
97}
98
99template <typename PointT> void
100Pyramid<PointT>::compute (std::vector<PointCloudPtr>& output)
101{
102 std::cout << "compute" << std::endl;
103 if (!initCompute ())
104 {
105 PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
106 return;
107 }
108
109 int kernel_rows = static_cast<int> (kernel_.rows ());
110 int kernel_cols = static_cast<int> (kernel_.cols ());
111 int kernel_center_x = kernel_cols / 2;
112 int kernel_center_y = kernel_rows / 2;
113
114 output.resize (levels_ + 1);
115 output[0].reset (new pcl::PointCloud<PointT>);
116 *(output[0]) = *input_;
117
118 if (input_->is_dense)
119 {
120 for (int l = 1; l <= levels_; ++l)
121 {
122 output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
123 const PointCloud<PointT> &previous = *output[l-1];
124 PointCloud<PointT> &next = *output[l];
125#pragma omp parallel for \
126 default(none) \
127 shared(next) \
128 num_threads(threads_)
129 for(int i=0; i < next.height; ++i)
130 {
131 for(int j=0; j < next.width; ++j)
132 {
133 for(int m=0; m < kernel_rows; ++m)
134 {
135 int mm = kernel_rows - 1 - m;
136 for(int n=0; n < kernel_cols; ++n)
137 {
138 int nn = kernel_cols - 1 - n;
139
140 int ii = 2*i + (m - kernel_center_y);
141 int jj = 2*j + (n - kernel_center_x);
142
143 if (ii < 0) ii = 0;
144 if (ii >= previous.height) ii = previous.height - 1;
145 if (jj < 0) jj = 0;
146 if (jj >= previous.width) jj = previous.width - 1;
147 next.at (j,i) += previous.at (jj,ii) * kernel_ (mm,nn);
148 }
149 }
150 }
151 }
152 }
153 }
154 else
155 {
156 for (int l = 1; l <= levels_; ++l)
157 {
158 output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
159 const PointCloud<PointT> &previous = *output[l-1];
160 PointCloud<PointT> &next = *output[l];
161#pragma omp parallel for \
162 default(none) \
163 shared(next) \
164 num_threads(threads_)
165 for(int i=0; i < next.height; ++i)
166 {
167 for(int j=0; j < next.width; ++j)
168 {
169 float weight = 0;
170 for(int m=0; m < kernel_rows; ++m)
171 {
172 int mm = kernel_rows - 1 - m;
173 for(int n=0; n < kernel_cols; ++n)
174 {
175 int nn = kernel_cols - 1 - n;
176 int ii = 2*i + (m - kernel_center_y);
177 int jj = 2*j + (n - kernel_center_x);
178 if (ii < 0) ii = 0;
179 if (ii >= previous.height) ii = previous.height - 1;
180 if (jj < 0) jj = 0;
181 if (jj >= previous.width) jj = previous.width - 1;
182 if (!isFinite (previous.at (jj,ii)))
183 continue;
184 if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
185 {
186 next.at (j,i) += previous.at (jj,ii).x * kernel_ (mm,nn);
187 weight+= kernel_ (mm,nn);
188 }
189 }
190 }
191 if (weight == 0)
192 nullify (next.at (j,i));
193 else
194 {
195 weight = 1.f/weight;
196 next.at (j,i)*= weight;
197 }
198 }
199 }
200 }
201 }
202}
203
204template <> void
206
207template <> void
209
210template<> void
212{
213 p.r = 0; p.g = 0; p.b = 0;
214}
215
216template <> void
218
219} // namespace filters
220} // namespace pcl
221
222#endif
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.
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).
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:100
typename PointCloud< PointT >::Ptr PointCloudPtr
Definition pyramid.h:65
Define standard C methods to do distance calculations.
Defines all the PCL implemented PointT point type structures.
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.