40 #ifndef PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
41 #define PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
43 #include <pcl/search/organized.h>
44 #include <pcl/search/kdtree.h>
45 #include <pcl/pcl_config.h>
46 #include <pcl/point_types.h>
47 #include <pcl/common/point_tests.h>
59 template <
typename Po
intT>
65 n.normal_x = n.normal_y = n.normal_z = std::numeric_limits<float>::quiet_NaN ();
69 template <
typename Po
intT>
class
75 p.
x = p.
y = std::numeric_limits<float>::quiet_NaN ();
82 template<
typename Po
intInT,
typename Po
intOutT>
bool
87 PCL_ERROR (
"Sigma is not set or equal to 0!\n", sigma_);
90 sigma_sqr_ = sigma_ * sigma_;
92 if (sigma_coefficient_)
94 if ((*sigma_coefficient_) > 6 || (*sigma_coefficient_) < 3)
96 PCL_ERROR (
"Sigma coefficient (%f) out of [3..6]!\n", (*sigma_coefficient_));
100 threshold_ = (*sigma_coefficient_) * (*sigma_coefficient_) * sigma_sqr_;
107 template<
typename Po
intInT,
typename Po
intOutT> PointOutT
109 const std::vector<float>& distances)
113 float total_weight = 0;
114 std::vector<float>::const_iterator dist_it = distances.begin ();
116 for (Indices::const_iterator idx_it = indices.begin ();
117 idx_it != indices.end ();
120 if (*dist_it <= threshold_ &&
isFinite ((*input_) [*idx_it]))
122 float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
123 result += weight * (*input_) [*idx_it];
124 total_weight += weight;
127 if (total_weight != 0)
128 result /= total_weight;
130 makeInfinite (result);
136 template<
typename Po
intInT,
typename Po
intOutT> PointOutT
141 float total_weight = 0;
142 float r = 0, g = 0, b = 0;
143 std::vector<float>::const_iterator dist_it = distances.begin ();
145 for (Indices::const_iterator idx_it = indices.begin ();
146 idx_it != indices.end ();
149 if (*dist_it <= threshold_ &&
isFinite ((*input_) [*idx_it]))
151 float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
152 result.x += weight * (*input_) [*idx_it].x;
153 result.y += weight * (*input_) [*idx_it].y;
154 result.z += weight * (*input_) [*idx_it].z;
155 r += weight *
static_cast<float> ((*input_) [*idx_it].r);
156 g += weight *
static_cast<float> ((*input_) [*idx_it].g);
157 b += weight *
static_cast<float> ((*input_) [*idx_it].b);
158 total_weight += weight;
161 if (total_weight != 0)
163 total_weight = 1.f/total_weight;
164 r*= total_weight; g*= total_weight; b*= total_weight;
165 result.x*= total_weight; result.y*= total_weight; result.z*= total_weight;
166 result.r =
static_cast<std::uint8_t
> (r);
167 result.g =
static_cast<std::uint8_t
> (g);
168 result.b =
static_cast<std::uint8_t
> (b);
171 makeInfinite (result);
177 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
186 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
bool
191 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] init failed!\n");
197 if (input_->isOrganized ())
206 tree_->setInputCloud (surface_);
208 if (search_radius_ <= 0.0)
210 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0\n",
217 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] init failed : ");
218 PCL_ERROR (
"kernel_ must implement ConvolvingKernel interface\n!");
221 kernel_.setInputCloud (surface_);
223 if (!kernel_.initCompute ())
225 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n");
232 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
void
237 PCL_ERROR (
"[pcl::filters::Convlution3D::convolve] init failed!\n");
240 output.
resize (surface_->size ());
241 output.
width = surface_->width;
242 output.
height = surface_->height;
243 output.
is_dense = surface_->is_dense;
245 std::vector<float> nn_distances;
247 #pragma omp parallel for \
250 firstprivate(nn_indices, nn_distances) \
251 num_threads(threads_)
252 for (std::int64_t point_idx = 0; point_idx < static_cast<std::int64_t> (surface_->size ()); ++point_idx)
254 const PointInT& point_in = surface_->points [point_idx];
255 PointOutT& point_out = output [point_idx];
257 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances))
259 point_out = kernel_ (nn_indices, nn_distances);
263 kernel_.makeInfinite (point_out);
A point structure representing normal coordinates and the surface curvature estimate.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
A 2D point structure representing Euclidean xy coordinates.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Convolution3D()
Constructor.
static void makeInfinite(PointOutT &p)
Utility function that annihilates a point making it fail the pcl::isFinite test.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t height
The point cloud height (if organized as an image-structure).
bool initCompute()
initialize computation
IndicesAllocator<> Indices
Type used for indices in PCL.
bool initCompute()
Must call this method before doing any computation.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Class ConvolvingKernel base class for all convolving kernels.
void convolve(PointCloudOut &output)
Convolve point cloud.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
A point structure representing Euclidean xyz coordinates, and the RGB color.
PointOutT operator()(const Indices &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
virtual PointOutT operator()(const Indices &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.