41 #ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_
42 #define PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_
44 #include <pcl/common/io.h>
50 template <
typename Po
intT>
void
53 if (!input_->isOrganized ())
55 PCL_ERROR (
"[pcl::FastBilateralFilter] Input cloud needs to be organized.\n");
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)
65 for (std::size_t y = 0; y < output.
height; ++y)
67 if (std::isfinite (output (x, y).z))
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;
79 PCL_WARN (
"[pcl::FastBilateralFilter] Given an empty cloud. Doing nothing.\n");
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;
88 const float base_delta = base_max - base_min;
90 const std::size_t padding_xy = 2;
91 const std::size_t padding_z = 2;
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;
98 Array3D data (small_width, small_height, small_depth);
99 for (std::size_t x = 0; x < input_->width; ++x)
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)
104 const float z = output (x,y).z - base_min;
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;
109 Eigen::Vector2f& d = data (small_x, small_y, small_z);
110 d[0] += output (x,y).z;
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));
121 Array3D buffer (small_width, small_height, small_depth);
123 for (std::size_t dim = 0; dim < 3; ++dim)
125 const long int off = offset[dim];
126 for (std::size_t n_iter = 0; n_iter < 2; ++n_iter)
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)
132 Eigen::Vector2f* d_ptr = &(data (x,y,1));
133 Eigen::Vector2f* b_ptr = &(buffer (x,y,1));
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;
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;
146 for (std::size_t x = 0; x < input_->width; x++)
147 for (std::size_t y = 0; y < input_->height; y++)
149 const float z = output (x,y).z - base_min;
151 static_cast<float> (y) / sigma_s_ + padding_xy,
152 z / sigma_r_ + padding_z);
153 output(x,y).z = D[0];
158 for (std::size_t x = 0; x < input_->width; ++x)
159 for (std::size_t y = 0; y < input_->height; ++y)
161 const float z = output (x,y).z - base_min;
163 static_cast<float> (y) / sigma_s_ + padding_xy,
164 z / sigma_r_ + padding_z);
165 output (x,y).z = D[0] / D[1];
171 template <
typename Po
intT> std::size_t
173 const std::size_t max_value,
176 if (x >= min_value && x <= max_value)
188 template <
typename Po
intT> Eigen::Vector2f
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);
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);
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);
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);
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);
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).
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.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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)