Point Cloud Library (PCL)  1.11.0
fast_bilateral.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$
38  *
39  */
40 
41 #ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_
42 #define PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_
43 
44 #include <pcl/common/io.h>
45 
46 
47 namespace pcl
48 {
49 
50 template <typename PointT> void
52 {
53  if (!input_->isOrganized ())
54  {
55  PCL_ERROR ("[pcl::FastBilateralFilter] Input cloud needs to be organized.\n");
56  return;
57  }
58 
59  copyPointCloud (*input_, output);
60  float base_max = -std::numeric_limits<float>::max (),
61  base_min = std::numeric_limits<float>::max ();
62  bool found_finite = false;
63  for (std::size_t x = 0; x < output.width; ++x)
64  {
65  for (std::size_t y = 0; y < output.height; ++y)
66  {
67  if (std::isfinite (output (x, y).z))
68  {
69  if (base_max < output (x, y).z)
70  base_max = output (x, y).z;
71  if (base_min > output (x, y).z)
72  base_min = output (x, y).z;
73  found_finite = true;
74  }
75  }
76  }
77  if (!found_finite)
78  {
79  PCL_WARN ("[pcl::FastBilateralFilter] Given an empty cloud. Doing nothing.\n");
80  return;
81  }
82 
83  for (std::size_t x = 0; x < output.width; ++x)
84  for (std::size_t y = 0; y < output.height; ++y)
85  if (!std::isfinite (output (x, y).z))
86  output (x, y).z = base_max;
87 
88  const float base_delta = base_max - base_min;
89 
90  const std::size_t padding_xy = 2;
91  const std::size_t padding_z = 2;
92 
93  const std::size_t small_width = static_cast<std::size_t> (static_cast<float> (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy;
94  const std::size_t small_height = static_cast<std::size_t> (static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy;
95  const std::size_t small_depth = static_cast<std::size_t> (base_delta / sigma_r_) + 1 + 2 * padding_z;
96 
97 
98  Array3D data (small_width, small_height, small_depth);
99  for (std::size_t x = 0; x < input_->width; ++x)
100  {
101  const std::size_t small_x = static_cast<std::size_t> (static_cast<float> (x) / sigma_s_ + 0.5f) + padding_xy;
102  for (std::size_t y = 0; y < input_->height; ++y)
103  {
104  const float z = output (x,y).z - base_min;
105 
106  const std::size_t small_y = static_cast<std::size_t> (static_cast<float> (y) / sigma_s_ + 0.5f) + padding_xy;
107  const std::size_t small_z = static_cast<std::size_t> (static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z;
108 
109  Eigen::Vector2f& d = data (small_x, small_y, small_z);
110  d[0] += output (x,y).z;
111  d[1] += 1.0f;
112  }
113  }
114 
115 
116  std::vector<long int> offset (3);
117  offset[0] = &(data (1,0,0)) - &(data (0,0,0));
118  offset[1] = &(data (0,1,0)) - &(data (0,0,0));
119  offset[2] = &(data (0,0,1)) - &(data (0,0,0));
120 
121  Array3D buffer (small_width, small_height, small_depth);
122 
123  for (std::size_t dim = 0; dim < 3; ++dim)
124  {
125  const long int off = offset[dim];
126  for (std::size_t n_iter = 0; n_iter < 2; ++n_iter)
127  {
128  std::swap (buffer, data);
129  for(std::size_t x = 1; x < small_width - 1; ++x)
130  for(std::size_t y = 1; y < small_height - 1; ++y)
131  {
132  Eigen::Vector2f* d_ptr = &(data (x,y,1));
133  Eigen::Vector2f* b_ptr = &(buffer (x,y,1));
134 
135  for(std::size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
136  *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
137  }
138  }
139  }
140 
141  if (early_division_)
142  {
143  for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.begin (); d != data.end (); ++d)
144  *d /= ((*d)[0] != 0) ? (*d)[1] : 1;
145 
146  for (std::size_t x = 0; x < input_->width; x++)
147  for (std::size_t y = 0; y < input_->height; y++)
148  {
149  const float z = output (x,y).z - base_min;
150  const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
151  static_cast<float> (y) / sigma_s_ + padding_xy,
152  z / sigma_r_ + padding_z);
153  output(x,y).z = D[0];
154  }
155  }
156  else
157  {
158  for (std::size_t x = 0; x < input_->width; ++x)
159  for (std::size_t y = 0; y < input_->height; ++y)
160  {
161  const float z = output (x,y).z - base_min;
162  const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
163  static_cast<float> (y) / sigma_s_ + padding_xy,
164  z / sigma_r_ + padding_z);
165  output (x,y).z = D[0] / D[1];
166  }
167  }
168 }
169 
170 
171 template <typename PointT> std::size_t
172 FastBilateralFilter<PointT>::Array3D::clamp (const std::size_t min_value,
173  const std::size_t max_value,
174  const std::size_t x)
175 {
176  if (x >= min_value && x <= max_value)
177  {
178  return x;
179  }
180  if (x < min_value)
181  {
182  return (min_value);
183  }
184  return (max_value);
185 }
186 
187 
188 template <typename PointT> Eigen::Vector2f
190  const float y,
191  const float z)
192 {
193  const std::size_t x_index = clamp (0, x_dim_ - 1, static_cast<std::size_t> (x));
194  const std::size_t xx_index = clamp (0, x_dim_ - 1, x_index + 1);
195 
196  const std::size_t y_index = clamp (0, y_dim_ - 1, static_cast<std::size_t> (y));
197  const std::size_t yy_index = clamp (0, y_dim_ - 1, y_index + 1);
198 
199  const std::size_t z_index = clamp (0, z_dim_ - 1, static_cast<std::size_t> (z));
200  const std::size_t zz_index = clamp (0, z_dim_ - 1, z_index + 1);
201 
202  const float x_alpha = x - static_cast<float> (x_index);
203  const float y_alpha = y - static_cast<float> (y_index);
204  const float z_alpha = z - static_cast<float> (z_index);
205 
206  return
207  (1.0f-x_alpha) * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(x_index, y_index, z_index) +
208  x_alpha * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(xx_index, y_index, z_index) +
209  (1.0f-x_alpha) * y_alpha * (1.0f-z_alpha) * (*this)(x_index, yy_index, z_index) +
210  x_alpha * y_alpha * (1.0f-z_alpha) * (*this)(xx_index, yy_index, z_index) +
211  (1.0f-x_alpha) * (1.0f-y_alpha) * z_alpha * (*this)(x_index, y_index, zz_index) +
212  x_alpha * (1.0f-y_alpha) * z_alpha * (*this)(xx_index, y_index, zz_index) +
213  (1.0f-x_alpha) * y_alpha * z_alpha * (*this)(x_index, yy_index, zz_index) +
214  x_alpha * y_alpha * z_alpha * (*this)(xx_index, yy_index, zz_index);
215 }
216 
217 } // namespace pcl
218 
219 #endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ */
220 
Eigen::Vector2f trilinear_interpolation(const float x, const float y, const float z)
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > >::iterator end()
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
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:121
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
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) override
Filter the input data and store the results into output.
static std::size_t clamp(const std::size_t min_value, const std::size_t max_value, const std::size_t x)