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>
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);
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
bool initCompute()
initialize computation
void convolve(PointCloudOut &output)
Convolve point cloud.
Convolution3D()
Constructor.
Class ConvolvingKernel base class for all convolving kernels.
static void makeInfinite(PointOutT &p)
Utility function that annihilates a point making it fail the pcl::isFinite test.
virtual PointOutT operator()(const Indices &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
bool initCompute()
Must call this method before doing any computation.
PointOutT operator()(const Indices &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
Defines all the PCL implemented PointT point type structures.
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...
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing normal coordinates and the surface curvature estimate.
A 2D point structure representing Euclidean xy coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.