Point Cloud Library (PCL) 1.12.0
fast_bilateral_omp.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 * Copyright (c) 2004, Sylvain Paris and Francois Sillion
7
8 * All rights reserved.
9
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id: fast_bilateral_omp.hpp 8381 2013-01-02 23:12:44Z sdmiller $
38 *
39 */
40#ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_
41#define PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_
42
43#include <pcl/filters/fast_bilateral_omp.h>
44#include <pcl/common/io.h>
45
46//////////////////////////////////////////////////////////////////////////////////////////////
47template <typename PointT> void
49{
50 if (nr_threads == 0)
51#ifdef _OPENMP
52 threads_ = omp_get_num_procs();
53#else
54 threads_ = 1;
55#endif
56 else
57 threads_ = nr_threads;
58}
59
60//////////////////////////////////////////////////////////////////////////////////////////////
61template <typename PointT> void
63{
64 if (!input_->isOrganized ())
65 {
66 PCL_ERROR ("[pcl::FastBilateralFilterOMP] Input cloud needs to be organized.\n");
67 return;
68 }
69
70 copyPointCloud (*input_, output);
71 float base_max = -std::numeric_limits<float>::max (),
72 base_min = std::numeric_limits<float>::max ();
73 bool found_finite = false;
74 for (const auto& pt: output)
75 {
76 if (std::isfinite(pt.z))
77 {
78 base_max = std::max<float>(pt.z, base_max);
79 base_min = std::min<float>(pt.z, base_min);
80 found_finite = true;
81 }
82 }
83 if (!found_finite)
84 {
85 PCL_WARN ("[pcl::FastBilateralFilterOMP] Given an empty cloud. Doing nothing.\n");
86 return;
87 }
88#pragma omp parallel for \
89 default(none) \
90 shared(base_min, base_max, output) \
91 num_threads(threads_)
92 for (long int i = 0; i < static_cast<long int> (output.size ()); ++i)
93 if (!std::isfinite (output.at(i).z))
94 output.at(i).z = base_max;
95
96 const float base_delta = base_max - base_min;
97
98 const std::size_t padding_xy = 2;
99 const std::size_t padding_z = 2;
100
101 const std::size_t small_width = static_cast<std::size_t> (static_cast<float> (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy;
102 const std::size_t small_height = static_cast<std::size_t> (static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy;
103 const std::size_t small_depth = static_cast<std::size_t> (base_delta / sigma_r_) + 1 + 2 * padding_z;
104
105 Array3D data (small_width, small_height, small_depth);
106#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
107#pragma omp parallel for \
108 default(none) \
109 shared(base_min, data, output) \
110 num_threads(threads_)
111#else
112#pragma omp parallel for \
113 default(none) \
114 shared(base_min, data, output, small_height, small_width) \
115 num_threads(threads_)
116#endif
117 for (long int i = 0; i < static_cast<long int> (small_width * small_height); ++i)
118 {
119 std::size_t small_x = static_cast<std::size_t> (i % small_width);
120 std::size_t small_y = static_cast<std::size_t> (i / small_width);
121 std::size_t start_x = static_cast<std::size_t>(
122 std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f));
123 std::size_t end_x = static_cast<std::size_t>(
124 std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f));
125 std::size_t start_y = static_cast<std::size_t>(
126 std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f));
127 std::size_t end_y = static_cast<std::size_t>(
128 std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f));
129 for (std::size_t x = start_x; x < end_x && x < input_->width; ++x)
130 {
131 for (std::size_t y = start_y; y < end_y && y < input_->height; ++y)
132 {
133 const float z = output (x,y).z - base_min;
134 const std::size_t small_z = static_cast<std::size_t> (static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z;
135 Eigen::Vector2f& d = data (small_x, small_y, small_z);
136 d[0] += output (x,y).z;
137 d[1] += 1.0f;
138 }
139 }
140 }
141
142 std::vector<long int> offset (3);
143 offset[0] = &(data (1,0,0)) - &(data (0,0,0));
144 offset[1] = &(data (0,1,0)) - &(data (0,0,0));
145 offset[2] = &(data (0,0,1)) - &(data (0,0,0));
146
147 Array3D buffer (small_width, small_height, small_depth);
148
149 for (std::size_t dim = 0; dim < 3; ++dim)
150 {
151 for (std::size_t n_iter = 0; n_iter < 2; ++n_iter)
152 {
153 Array3D* current_buffer = (n_iter % 2 == 1 ? &buffer : &data);
154 Array3D* current_data =(n_iter % 2 == 1 ? &data : &buffer);
155#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
156#pragma omp parallel for \
157 default(none) \
158 shared(current_buffer, current_data, dim, offset) \
159 num_threads(threads_)
160#else
161#pragma omp parallel for \
162 default(none) \
163 shared(current_buffer, current_data, dim, offset, small_depth, small_height, small_width) \
164 num_threads(threads_)
165#endif
166 for(long int i = 0; i < static_cast<long int> ((small_width - 2)*(small_height - 2)); ++i)
167 {
168 std::size_t x = static_cast<std::size_t> (i % (small_width - 2) + 1);
169 std::size_t y = static_cast<std::size_t> (i / (small_width - 2) + 1);
170 const long int off = offset[dim];
171 Eigen::Vector2f* d_ptr = &(current_data->operator() (x,y,1));
172 Eigen::Vector2f* b_ptr = &(current_buffer->operator() (x,y,1));
173
174 for(std::size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
175 *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
176 }
177 }
178 }
179 // Note: this works because there are an even number of iterations.
180 // If there were an odd number, we would need to end with a:
181 // std::swap (data, buffer);
182
183 if (early_division_)
184 {
185 for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.begin (); d != data.end (); ++d)
186 *d /= ((*d)[0] != 0) ? (*d)[1] : 1;
187
188#pragma omp parallel for \
189 default(none) \
190 shared(base_min, data, output) \
191 num_threads(threads_)
192 for (long int i = 0; i < static_cast<long int> (input_->size ()); ++i)
193 {
194 std::size_t x = static_cast<std::size_t> (i % input_->width);
195 std::size_t y = static_cast<std::size_t> (i / input_->width);
196 const float z = output (x,y).z - base_min;
197 const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
198 static_cast<float> (y) / sigma_s_ + padding_xy,
199 z / sigma_r_ + padding_z);
200 output(x,y).z = D[0];
201 }
202 }
203 else
204 {
205#pragma omp parallel for \
206 default(none) \
207 shared(base_min, data, output) \
208 num_threads(threads_)
209 for (long i = 0; i < static_cast<long int> (input_->size ()); ++i)
210 {
211 std::size_t x = static_cast<std::size_t> (i % input_->width);
212 std::size_t y = static_cast<std::size_t> (i / input_->width);
213 const float z = output (x,y).z - base_min;
214 const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
215 static_cast<float> (y) / sigma_s_ + padding_xy,
216 z / sigma_r_ + padding_z);
217 output (x,y).z = D[0] / D[1];
218 }
219 }
220}
221
222
223
224#endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ */
225
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
void applyFilter(PointCloud &output) override
Filter the input data and store the results into output.
typename FastBilateralFilter< PointT >::Array3D Array3D
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:262
std::size_t size() const
Definition: point_cloud.h:443
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:144