Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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
47namespace pcl
48{
49
50template <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 (const auto& pt: output)
64 {
65 if (std::isfinite(pt.z))
66 {
67 base_max = std::max<float>(pt.z, base_max);
68 base_min = std::min<float>(pt.z, base_min);
69 found_finite = true;
70 }
71 }
72 if (!found_finite)
73 {
74 PCL_WARN ("[pcl::FastBilateralFilter] Given an empty cloud. Doing nothing.\n");
75 return;
76 }
77
78 for (auto& pt: output)
79 {
80 if (!std::isfinite(pt.z))
81 {
82 pt.z = base_max;
83 }
84 }
85
86 const float base_delta = base_max - base_min;
87
88 const std::size_t padding_xy = 2;
89 const std::size_t padding_z = 2;
90
91 const std::size_t small_width = static_cast<std::size_t> (static_cast<float> (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy;
92 const std::size_t small_height = static_cast<std::size_t> (static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy;
93 const std::size_t small_depth = static_cast<std::size_t> (base_delta / sigma_r_) + 1 + 2 * padding_z;
94
95
96 Array3D data (small_width, small_height, small_depth);
97 for (std::size_t x = 0; x < input_->width; ++x)
98 {
99 const std::size_t small_x = static_cast<std::size_t> (static_cast<float> (x) / sigma_s_ + 0.5f) + padding_xy;
100 for (std::size_t y = 0; y < input_->height; ++y)
101 {
102 const float z = output (x,y).z - base_min;
103
104 const std::size_t small_y = static_cast<std::size_t> (static_cast<float> (y) / sigma_s_ + 0.5f) + padding_xy;
105 const std::size_t small_z = static_cast<std::size_t> (static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z;
106
107 Eigen::Vector2f& d = data (small_x, small_y, small_z);
108 d[0] += output (x,y).z;
109 d[1] += 1.0f;
110 }
111 }
112
113
114 std::vector<long int> offset (3);
115 offset[0] = &(data (1,0,0)) - &(data (0,0,0));
116 offset[1] = &(data (0,1,0)) - &(data (0,0,0));
117 offset[2] = &(data (0,0,1)) - &(data (0,0,0));
118
119 Array3D buffer (small_width, small_height, small_depth);
120
121 for (std::size_t dim = 0; dim < 3; ++dim)
122 {
123 const long int off = offset[dim];
124 for (std::size_t n_iter = 0; n_iter < 2; ++n_iter)
125 {
126 std::swap (buffer, data);
127 for(std::size_t x = 1; x < small_width - 1; ++x)
128 for(std::size_t y = 1; y < small_height - 1; ++y)
129 {
130 Eigen::Vector2f* d_ptr = &(data (x,y,1));
131 Eigen::Vector2f* b_ptr = &(buffer (x,y,1));
132
133 for(std::size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
134 *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
135 }
136 }
137 }
138
139 if (early_division_)
140 {
141 for (auto d = data.begin (); d != data.end (); ++d)
142 *d /= ((*d)[0] != 0) ? (*d)[1] : 1;
143
144 for (std::size_t x = 0; x < input_->width; x++)
145 for (std::size_t y = 0; y < input_->height; y++)
146 {
147 const float z = output (x,y).z - base_min;
148 const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
149 static_cast<float> (y) / sigma_s_ + padding_xy,
150 z / sigma_r_ + padding_z);
151 output(x,y).z = D[0];
152 }
153 }
154 else
155 {
156 for (std::size_t x = 0; x < input_->width; ++x)
157 for (std::size_t y = 0; y < input_->height; ++y)
158 {
159 const float z = output (x,y).z - base_min;
160 const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
161 static_cast<float> (y) / sigma_s_ + padding_xy,
162 z / sigma_r_ + padding_z);
163 output (x,y).z = D[0] / D[1];
164 }
165 }
166}
167
168
169template <typename PointT> std::size_t
171 const std::size_t max_value,
172 const std::size_t x)
173{
174 if (x >= min_value && x <= max_value)
175 {
176 return x;
177 }
178 if (x < min_value)
179 {
180 return (min_value);
181 }
182 return (max_value);
183}
184
185
186template <typename PointT> Eigen::Vector2f
188 const float y,
189 const float z)
190{
191 const std::size_t x_index = clamp (0, x_dim_ - 1, static_cast<std::size_t> (x));
192 const std::size_t xx_index = clamp (0, x_dim_ - 1, x_index + 1);
193
194 const std::size_t y_index = clamp (0, y_dim_ - 1, static_cast<std::size_t> (y));
195 const std::size_t yy_index = clamp (0, y_dim_ - 1, y_index + 1);
196
197 const std::size_t z_index = clamp (0, z_dim_ - 1, static_cast<std::size_t> (z));
198 const std::size_t zz_index = clamp (0, z_dim_ - 1, z_index + 1);
199
200 const float x_alpha = x - static_cast<float> (x_index);
201 const float y_alpha = y - static_cast<float> (y_index);
202 const float z_alpha = z - static_cast<float> (z_index);
203
204 return
205 (1.0f-x_alpha) * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(x_index, y_index, z_index) +
206 x_alpha * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(xx_index, y_index, z_index) +
207 (1.0f-x_alpha) * y_alpha * (1.0f-z_alpha) * (*this)(x_index, yy_index, z_index) +
208 x_alpha * y_alpha * (1.0f-z_alpha) * (*this)(xx_index, yy_index, z_index) +
209 (1.0f-x_alpha) * (1.0f-y_alpha) * z_alpha * (*this)(x_index, y_index, zz_index) +
210 x_alpha * (1.0f-y_alpha) * z_alpha * (*this)(xx_index, y_index, zz_index) +
211 (1.0f-x_alpha) * y_alpha * z_alpha * (*this)(x_index, yy_index, zz_index) +
212 x_alpha * y_alpha * z_alpha * (*this)(xx_index, yy_index, zz_index);
213}
214
215} // namespace pcl
216
217#endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ */
218
static std::size_t clamp(const std::size_t min_value, const std::size_t max_value, const std::size_t x)
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > >::iterator end()
Eigen::Vector2f trilinear_interpolation(const float x, const float y, const float z)
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.
typename Filter< PointT >::PointCloud PointCloud
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:142