40 #ifndef PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
41 #define PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
43 #include <pcl/pcl_config.h>
44 #include <pcl/point_types.h>
56 template <
typename Po
intT>
62 n.normal_x = n.normal_y = n.normal_z = std::numeric_limits<float>::quiet_NaN ();
66 template <
typename Po
intT>
class
72 p.
x = p.
y = std::numeric_limits<float>::quiet_NaN ();
79 template<
typename Po
intInT,
typename Po
intOutT>
bool
84 PCL_ERROR (
"Sigma is not set or equal to 0!\n", sigma_);
87 sigma_sqr_ = sigma_ * sigma_;
89 if (sigma_coefficient_)
91 if ((*sigma_coefficient_) > 6 || (*sigma_coefficient_) < 3)
93 PCL_ERROR (
"Sigma coefficient (%f) out of [3..6]!\n", (*sigma_coefficient_));
97 threshold_ = (*sigma_coefficient_) * (*sigma_coefficient_) * sigma_sqr_;
104 template<
typename Po
intInT,
typename Po
intOutT> PointOutT
106 const std::vector<float>& distances)
110 float total_weight = 0;
111 std::vector<float>::const_iterator dist_it = distances.begin ();
113 for (std::vector<int>::const_iterator idx_it = indices.begin ();
114 idx_it != indices.end ();
117 if (*dist_it <= threshold_ &&
isFinite ((*input_) [*idx_it]))
119 float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
120 result += weight * (*input_) [*idx_it];
121 total_weight += weight;
124 if (total_weight != 0)
125 result /= total_weight;
127 makeInfinite (result);
133 template<
typename Po
intInT,
typename Po
intOutT> PointOutT
138 float total_weight = 0;
139 float r = 0, g = 0, b = 0;
140 std::vector<float>::const_iterator dist_it = distances.begin ();
142 for (std::vector<int>::const_iterator idx_it = indices.begin ();
143 idx_it != indices.end ();
146 if (*dist_it <= threshold_ &&
isFinite ((*input_) [*idx_it]))
148 float weight = std::exp (-0.5f * (*dist_it) / sigma_sqr_);
149 result.x += weight * (*input_) [*idx_it].x;
150 result.y += weight * (*input_) [*idx_it].y;
151 result.z += weight * (*input_) [*idx_it].z;
152 r += weight *
static_cast<float> ((*input_) [*idx_it].r);
153 g += weight *
static_cast<float> ((*input_) [*idx_it].g);
154 b += weight *
static_cast<float> ((*input_) [*idx_it].b);
155 total_weight += weight;
158 if (total_weight != 0)
160 total_weight = 1.f/total_weight;
161 r*= total_weight; g*= total_weight; b*= total_weight;
162 result.x*= total_weight; result.y*= total_weight; result.z*= total_weight;
163 result.r =
static_cast<std::uint8_t
> (r);
164 result.g =
static_cast<std::uint8_t
> (g);
165 result.b =
static_cast<std::uint8_t
> (b);
168 makeInfinite (result);
174 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
183 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
bool
188 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] init failed!\n");
194 if (input_->isOrganized ())
203 tree_->setInputCloud (surface_);
205 if (search_radius_ <= 0.0)
207 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0",
214 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] init failed");
215 PCL_ERROR (
"kernel_ must implement ConvolvingKernel interface\n!");
218 kernel_.setInputCloud (surface_);
220 if (!kernel_.initCompute ())
222 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n");
229 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
void
234 PCL_ERROR (
"[pcl::filters::Convlution3D::convolve] init failed!\n");
237 output.
resize (surface_->size ());
238 output.
width = surface_->width;
239 output.
height = surface_->height;
240 output.
is_dense = surface_->is_dense;
241 std::vector<int> nn_indices;
242 std::vector<float> nn_distances;
244 #pragma omp parallel for \
247 private(nn_indices, nn_distances) \
248 num_threads(threads_)
249 for (std::int64_t point_idx = 0; point_idx < static_cast<std::int64_t> (surface_->size ()); ++point_idx)
251 const PointInT& point_in = surface_->points [point_idx];
252 PointOutT& point_out = output [point_idx];
254 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances))
256 point_out = kernel_ (nn_indices, nn_distances);
260 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...
virtual PointOutT operator()(const std::vector< int > &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
std::uint32_t width
The point cloud width (if organized as an image-structure).
A 2D point structure representing Euclidean xy coordinates.
Convolution3D()
Constructor.
static void makeInfinite(PointOutT &p)
Utility function that annihilates a point making it fail the pcl::isFinite test.
bool initCompute()
initialize computation
bool initCompute()
Must call this method before doing any computation.
Class ConvolvingKernel base class for all convolving kernels.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void convolve(PointCloudOut &output)
Convolve point cloud.
PointOutT operator()(const std::vector< int > &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
std::uint32_t height
The point cloud height (if organized as an image-structure).
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
A point structure representing Euclidean xyz coordinates, and the RGB color.
void resize(std::size_t n)
Resize the cloud.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...