40 #ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_
41 #define PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_
43 #include <pcl/filters/fast_bilateral_omp.h>
44 #include <pcl/common/io.h>
48 template <
typename Po
intT>
void
53 threads_ = omp_get_num_procs();
58 threads_ = nr_threads;
62 template <
typename Po
intT>
void
65 if (!input_->isOrganized ())
67 PCL_ERROR (
"[pcl::FastBilateralFilterOMP] Input cloud needs to be organized.\n");
72 float base_max = -std::numeric_limits<float>::max (),
73 base_min = std::numeric_limits<float>::max ();
74 bool found_finite =
false;
75 for (
const auto& pt: output)
77 if (std::isfinite(pt.z))
79 base_max = std::max<float>(pt.z, base_max);
80 base_min = std::min<float>(pt.z, base_min);
86 PCL_WARN (
"[pcl::FastBilateralFilterOMP] Given an empty cloud. Doing nothing.\n");
89 #pragma omp parallel for \
91 shared(base_min, base_max, output) \
93 for (
long int i = 0; i < static_cast<long int> (output.size ()); ++i)
94 if (!std::isfinite (output.at(i).z))
95 output.at(i).z = base_max;
97 const float base_delta = base_max - base_min;
99 const std::size_t padding_xy = 2;
100 const std::size_t padding_z = 2;
102 const std::size_t small_width =
static_cast<std::size_t
> (
static_cast<float> (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy;
103 const std::size_t small_height =
static_cast<std::size_t
> (
static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy;
104 const std::size_t small_depth =
static_cast<std::size_t
> (base_delta / sigma_r_) + 1 + 2 * padding_z;
106 Array3D data (small_width, small_height, small_depth);
107 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
108 #pragma omp parallel for \
110 shared(base_min, data, output) \
111 num_threads(threads_)
113 #pragma omp parallel for \
115 shared(base_min, data, output, small_height, small_width) \
116 num_threads(threads_)
118 for (
long int i = 0; i < static_cast<long int> (small_width * small_height); ++i)
120 std::size_t small_x =
static_cast<std::size_t
> (i % small_width);
121 std::size_t small_y =
static_cast<std::size_t
> (i / small_width);
122 std::size_t start_x =
static_cast<std::size_t
>(
123 std::max ((
static_cast<float> (small_x) -
static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f));
124 std::size_t end_x =
static_cast<std::size_t
>(
125 std::max ((
static_cast<float> (small_x) -
static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f));
126 std::size_t start_y =
static_cast<std::size_t
>(
127 std::max ((
static_cast<float> (small_y) -
static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f));
128 std::size_t end_y =
static_cast<std::size_t
>(
129 std::max ((
static_cast<float> (small_y) -
static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f));
130 for (std::size_t x = start_x; x < end_x && x < input_->width; ++x)
132 for (std::size_t y = start_y; y < end_y && y < input_->height; ++y)
134 const float z = output (x,y).z - base_min;
135 const std::size_t small_z =
static_cast<std::size_t
> (
static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z;
136 Eigen::Vector2f& d = data (small_x, small_y, small_z);
137 d[0] += output (x,y).z;
143 std::vector<long int> offset (3);
144 offset[0] = &(data (1,0,0)) - &(data (0,0,0));
145 offset[1] = &(data (0,1,0)) - &(data (0,0,0));
146 offset[2] = &(data (0,0,1)) - &(data (0,0,0));
148 Array3D buffer (small_width, small_height, small_depth);
150 for (std::size_t dim = 0; dim < 3; ++dim)
152 for (std::size_t n_iter = 0; n_iter < 2; ++n_iter)
154 Array3D* current_buffer = (n_iter % 2 == 1 ? &buffer : &data);
155 Array3D* current_data =(n_iter % 2 == 1 ? &data : &buffer);
156 #if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
157 #pragma omp parallel for \
159 shared(current_buffer, current_data, dim, offset) \
160 num_threads(threads_)
162 #pragma omp parallel for \
164 shared(current_buffer, current_data, dim, offset, small_depth, small_height, small_width) \
165 num_threads(threads_)
167 for(
long int i = 0; i < static_cast<long int> ((small_width - 2)*(small_height - 2)); ++i)
169 std::size_t x =
static_cast<std::size_t
> (i % (small_width - 2) + 1);
170 std::size_t y =
static_cast<std::size_t
> (i / (small_width - 2) + 1);
171 const long int off = offset[dim];
172 Eigen::Vector2f* d_ptr = &(current_data->operator() (x,y,1));
173 Eigen::Vector2f* b_ptr = &(current_buffer->operator() (x,y,1));
175 for(std::size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
176 *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
186 for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.begin (); d != data.end (); ++d)
187 *d /= ((*d)[0] != 0) ? (*d)[1] : 1;
189 #pragma omp parallel for \
191 shared(base_min, data, output) \
192 num_threads(threads_)
193 for (
long int i = 0; i < static_cast<long int> (input_->size ()); ++i)
195 std::size_t x =
static_cast<std::size_t
> (i % input_->width);
196 std::size_t y =
static_cast<std::size_t
> (i / input_->width);
197 const float z = output (x,y).z - base_min;
198 const Eigen::Vector2f D = data.trilinear_interpolation (
static_cast<float> (x) / sigma_s_ + padding_xy,
199 static_cast<float> (y) / sigma_s_ + padding_xy,
200 z / sigma_r_ + padding_z);
201 output(x,y).z = D[0];
206 #pragma omp parallel for \
208 shared(base_min, data, output) \
209 num_threads(threads_)
210 for (
long i = 0; i < static_cast<long int> (input_->size ()); ++i)
212 std::size_t x =
static_cast<std::size_t
> (i % input_->width);
213 std::size_t y =
static_cast<std::size_t
> (i / input_->width);
214 const float z = output (x,y).z - base_min;
215 const Eigen::Vector2f D = data.trilinear_interpolation (
static_cast<float> (x) / sigma_s_ + padding_xy,
216 static_cast<float> (y) / sigma_s_ + padding_xy,
217 z / sigma_r_ + padding_z);
218 output (x,y).z = D[0] / D[1];