Point Cloud Library (PCL)  1.9.0-dev
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 #include <pcl/console/time.h>
46 #include <assert.h>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT> void
51 {
52  if (nr_threads == 0)
53 #ifdef _OPENMP
54  threads_ = omp_get_num_procs();
55 #else
56  threads_ = 1;
57 #endif
58  else
59  threads_ = nr_threads;
60 }
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////
63 template <typename PointT> void
65 {
66  if (!input_->isOrganized ())
67  {
68  PCL_ERROR ("[pcl::FastBilateralFilterOMP] Input cloud needs to be organized.\n");
69  return;
70  }
71 
72  copyPointCloud (*input_, output);
73  float base_max = -std::numeric_limits<float>::max (),
74  base_min = std::numeric_limits<float>::max ();
75  bool found_finite = false;
76  for (size_t x = 0; x < output.width; ++x)
77  {
78  for (size_t y = 0; y < output.height; ++y)
79  {
80  if (pcl_isfinite (output (x, y).z))
81  {
82  if (base_max < output (x, y).z)
83  base_max = output (x, y).z;
84  if (base_min > output (x, y).z)
85  base_min = output (x, y).z;
86  found_finite = true;
87  }
88  }
89  }
90  if (!found_finite)
91  {
92  PCL_WARN ("[pcl::FastBilateralFilterOMP] Given an empty cloud. Doing nothing.\n");
93  return;
94  }
95 #ifdef _OPENMP
96 #pragma omp parallel for num_threads (threads_)
97 #endif
98  for (long int i = 0; i < static_cast<long int> (output.size ()); ++i)
99  if (!pcl_isfinite (output.at(i).z))
100  output.at(i).z = base_max;
101 
102  const float base_delta = base_max - base_min;
103 
104  const size_t padding_xy = 2;
105  const size_t padding_z = 2;
106 
107  const size_t small_width = static_cast<size_t> (static_cast<float> (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy;
108  const size_t small_height = static_cast<size_t> (static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy;
109  const size_t small_depth = static_cast<size_t> (base_delta / sigma_r_) + 1 + 2 * padding_z;
110 
111  Array3D data (small_width, small_height, small_depth);
112 #ifdef _OPENMP
113 #pragma omp parallel for num_threads (threads_)
114 #endif
115  for (long int i = 0; i < static_cast<long int> (small_width * small_height); ++i)
116  {
117  size_t small_x = static_cast<size_t> (i % small_width);
118  size_t small_y = static_cast<size_t> (i / small_width);
119  size_t start_x = static_cast<size_t>(
120  std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f));
121  size_t end_x = static_cast<size_t>(
122  std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f));
123  size_t start_y = static_cast<size_t>(
124  std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f));
125  size_t end_y = static_cast<size_t>(
126  std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f));
127  for (size_t x = start_x; x < end_x && x < input_->width; ++x)
128  {
129  for (size_t y = start_y; y < end_y && y < input_->height; ++y)
130  {
131  const float z = output (x,y).z - base_min;
132  const size_t small_z = static_cast<size_t> (static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z;
133  Eigen::Vector2f& d = data (small_x, small_y, small_z);
134  d[0] += output (x,y).z;
135  d[1] += 1.0f;
136  }
137  }
138  }
139 
140  std::vector<long int> offset (3);
141  offset[0] = &(data (1,0,0)) - &(data (0,0,0));
142  offset[1] = &(data (0,1,0)) - &(data (0,0,0));
143  offset[2] = &(data (0,0,1)) - &(data (0,0,0));
144 
145  Array3D buffer (small_width, small_height, small_depth);
146 
147  for (size_t dim = 0; dim < 3; ++dim)
148  {
149  for (size_t n_iter = 0; n_iter < 2; ++n_iter)
150  {
151  Array3D* current_buffer = (n_iter % 2 == 1 ? &buffer : &data);
152  Array3D* current_data =(n_iter % 2 == 1 ? &data : &buffer);
153 #ifdef _OPENMP
154 #pragma omp parallel for num_threads (threads_)
155 #endif
156  for(long int i = 0; i < static_cast<long int> ((small_width - 2)*(small_height - 2)); ++i)
157  {
158  size_t x = static_cast<size_t> (i % (small_width - 2) + 1);
159  size_t y = static_cast<size_t> (i / (small_width - 2) + 1);
160  const long int off = offset[dim];
161  Eigen::Vector2f* d_ptr = &(current_data->operator() (x,y,1));
162  Eigen::Vector2f* b_ptr = &(current_buffer->operator() (x,y,1));
163 
164  for(size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
165  *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
166  }
167  }
168  }
169  // Note: this works because there are an even number of iterations.
170  // If there were an odd number, we would need to end with a:
171  // std::swap (data, buffer);
172 
173  if (early_division_)
174  {
175  for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.begin (); d != data.end (); ++d)
176  *d /= ((*d)[0] != 0) ? (*d)[1] : 1;
177 
178 #ifdef _OPENMP
179 #pragma omp parallel for num_threads (threads_)
180 #endif
181  for (long int i = 0; i < static_cast<long int> (input_->size ()); ++i)
182  {
183  size_t x = static_cast<size_t> (i % input_->width);
184  size_t y = static_cast<size_t> (i / input_->width);
185  const float z = output (x,y).z - base_min;
186  const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
187  static_cast<float> (y) / sigma_s_ + padding_xy,
188  z / sigma_r_ + padding_z);
189  output(x,y).z = D[0];
190  }
191  }
192  else
193  {
194 #ifdef _OPENMP
195 #pragma omp parallel for num_threads (threads_)
196 #endif
197  for (long i = 0; i < static_cast<long int> (input_->size ()); ++i)
198  {
199  size_t x = static_cast<size_t> (i % input_->width);
200  size_t y = static_cast<size_t> (i / input_->width);
201  const float z = output (x,y).z - base_min;
202  const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
203  static_cast<float> (y) / sigma_s_ + padding_xy,
204  z / sigma_r_ + padding_z);
205  output (x,y).z = D[0] / D[1];
206  }
207  }
208 }
209 
210 
211 
212 #endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ */
213 
Eigen::Vector2f trilinear_interpolation(const float x, const float y, const float z)
size_t size() const
Definition: point_cloud.h:448
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > >::iterator end()
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
PointCloud represents the base class in PCL for storing collections of 3D points. ...
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > >::iterator begin()
void applyFilter(PointCloud &output)
Filter the input data and store the results into output.
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:283