40 #ifndef PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
41 #define PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
43 #include <pcl/pcl_config.h>
45 #include <pcl/common/point_operators.h>
57 template <
typename Po
intT>
63 n.normal_x = n.normal_y = n.normal_z = std::numeric_limits<float>::quiet_NaN ();
67 template <
typename Po
intT>
class
73 p.
x = p.
y = std::numeric_limits<float>::quiet_NaN ();
80 template<
typename Po
intInT,
typename Po
intOutT>
bool
85 PCL_ERROR (
"Sigma is not set or equal to 0!\n", sigma_);
88 sigma_sqr_ = sigma_ * sigma_;
90 if (sigma_coefficient_)
92 if ((*sigma_coefficient_) > 6 || (*sigma_coefficient_) < 3)
94 PCL_ERROR (
"Sigma coefficient (%f) out of [3..6]!\n", (*sigma_coefficient_));
98 threshold_ = (*sigma_coefficient_) * (*sigma_coefficient_) * sigma_sqr_;
105 template<
typename Po
intInT,
typename Po
intOutT> PointOutT
107 const std::vector<float>& distances)
111 float total_weight = 0;
112 std::vector<float>::const_iterator dist_it = distances.begin ();
114 for (std::vector<int>::const_iterator idx_it = indices.begin ();
115 idx_it != indices.end ();
118 if (*dist_it <= threshold_ &&
isFinite ((*input_) [*idx_it]))
120 float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
121 result += weight * (*input_) [*idx_it];
122 total_weight += weight;
125 if (total_weight != 0)
126 result /= total_weight;
128 makeInfinite (result);
134 template<
typename Po
intInT,
typename Po
intOutT> PointOutT
139 float total_weight = 0;
140 float r = 0, g = 0, b = 0;
141 std::vector<float>::const_iterator dist_it = distances.begin ();
143 for (std::vector<int>::const_iterator idx_it = indices.begin ();
144 idx_it != indices.end ();
147 if (*dist_it <= threshold_ &&
isFinite ((*input_) [*idx_it]))
149 float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
150 result.x += weight * (*input_) [*idx_it].x;
151 result.y += weight * (*input_) [*idx_it].y;
152 result.z += weight * (*input_) [*idx_it].z;
153 r += weight * static_cast<float> ((*input_) [*idx_it].r);
154 g += weight * static_cast<float> ((*input_) [*idx_it].g);
155 b += weight * static_cast<float> ((*input_) [*idx_it].b);
156 total_weight += weight;
159 if (total_weight != 0)
161 total_weight = 1.f/total_weight;
162 r*= total_weight; g*= total_weight; b*= total_weight;
163 result.x*= total_weight; result.y*= total_weight; result.z*= total_weight;
164 result.r = static_cast<std::uint8_t> (r);
165 result.g = static_cast<std::uint8_t> (g);
166 result.b = static_cast<std::uint8_t> (b);
169 makeInfinite (result);
175 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
184 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
bool
189 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] init failed!\n");
195 if (input_->isOrganized ())
204 tree_->setInputCloud (surface_);
206 if (search_radius_ <= 0.0)
208 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0",
215 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] init failed");
216 PCL_ERROR (
"kernel_ must implement ConvolvingKernel interface\n!");
219 kernel_.setInputCloud (surface_);
221 if (!kernel_.initCompute ())
223 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n");
230 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
void
235 PCL_ERROR (
"[pcl::filters::Convlution3D::convolve] init failed!\n");
238 output.
resize (surface_->size ());
239 output.
width = surface_->width;
240 output.
height = surface_->height;
241 output.
is_dense = surface_->is_dense;
242 std::vector<int> nn_indices;
243 std::vector<float> nn_distances;
246 #pragma omp parallel for shared (output) private (nn_indices, nn_distances) num_threads (threads_)
248 for (std::int64_t point_idx = 0; point_idx < static_cast<std::int64_t> (surface_->size ()); ++point_idx)
250 const PointInT& point_in = surface_->points [point_idx];
251 PointOutT& point_out = output [point_idx];
253 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances))
255 point_out = kernel_ (nn_indices, nn_distances);
259 kernel_.makeInfinite (point_out);