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>
45 #include <pcl/common/point_operators.h>
52 template <
typename Po
intT>
58 n.normal_x = n.normal_y = n.normal_z = std::numeric_limits<float>::quiet_NaN ();
62 template <
typename Po
intT>
class
68 p.
x = p.
y = std::numeric_limits<float>::quiet_NaN ();
75 template<
typename Po
intInT,
typename Po
intOutT>
bool
80 PCL_ERROR (
"Sigma is not set or equal to 0!\n", sigma_);
83 sigma_sqr_ = sigma_ * sigma_;
85 if (sigma_coefficient_)
87 if ((*sigma_coefficient_) > 6 || (*sigma_coefficient_) < 3)
89 PCL_ERROR (
"Sigma coefficient (%f) out of [3..6]!\n", (*sigma_coefficient_));
93 threshold_ = (*sigma_coefficient_) * (*sigma_coefficient_) * sigma_sqr_;
100 template<
typename Po
intInT,
typename Po
intOutT> PointOutT
102 const std::vector<float>& distances)
104 using namespace pcl::common;
106 float total_weight = 0;
107 std::vector<float>::const_iterator dist_it = distances.begin ();
109 for (std::vector<int>::const_iterator idx_it = indices.begin ();
110 idx_it != indices.end ();
113 if (*dist_it <= threshold_ &&
isFinite ((*input_) [*idx_it]))
115 float weight = expf (-0.5f * (*dist_it) / sigma_sqr_);
116 result += weight * (*input_) [*idx_it];
117 total_weight += weight;
120 if (total_weight != 0)
121 result /= total_weight;
123 makeInfinite (result);
129 template<
typename Po
intInT,
typename Po
intOutT> PointOutT
132 using namespace pcl::common;
134 float total_weight = 0;
135 float r = 0, g = 0, b = 0;
136 std::vector<float>::const_iterator dist_it = distances.begin ();
138 for (std::vector<int>::const_iterator idx_it = indices.begin ();
139 idx_it != indices.end ();
142 if (*dist_it <= threshold_ &&
isFinite ((*input_) [*idx_it]))
144 float weight = expf (-0.5f * (*dist_it) / sigma_sqr_);
145 result.x += weight * (*input_) [*idx_it].x;
146 result.y += weight * (*input_) [*idx_it].y;
147 result.z += weight * (*input_) [*idx_it].z;
148 r += weight *
static_cast<float> ((*input_) [*idx_it].r);
149 g += weight *
static_cast<float> ((*input_) [*idx_it].g);
150 b += weight *
static_cast<float> ((*input_) [*idx_it].b);
151 total_weight += weight;
154 if (total_weight != 0)
156 total_weight = 1.f/total_weight;
157 r*= total_weight; g*= total_weight; b*= total_weight;
158 result.x*= total_weight; result.y*= total_weight; result.z*= total_weight;
159 result.r =
static_cast<pcl::uint8_t
> (r);
160 result.g =
static_cast<pcl::uint8_t
> (g);
161 result.b =
static_cast<pcl::uint8_t
> (b);
164 makeInfinite (result);
170 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
179 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
bool
184 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] init failed!\n");
190 if (input_->isOrganized ())
199 tree_->setInputCloud (surface_);
201 if (search_radius_ <= 0.0)
203 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] search radius (%f) must be > 0",
210 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] init failed");
211 PCL_ERROR (
"kernel_ must implement ConvolvingKernel interface\n!");
214 kernel_.setInputCloud (surface_);
216 if (!kernel_.initCompute ())
218 PCL_ERROR (
"[pcl::filters::Convlution3D::initCompute] kernel initialization failed!\n");
225 template <
typename Po
intInT,
typename Po
intOutT,
typename KernelT>
void
230 PCL_ERROR (
"[pcl::filters::Convlution3D::convolve] init failed!\n");
233 output.
resize (surface_->size ());
234 output.
width = surface_->width;
235 output.
height = surface_->height;
236 output.
is_dense = surface_->is_dense;
237 std::vector<int> nn_indices;
238 std::vector<float> nn_distances;
241 #pragma omp parallel for shared (output) private (nn_indices, nn_distances) num_threads (threads_)
243 for (std::size_t point_idx = 0; point_idx < surface_->size (); ++point_idx)
245 const PointInT& point_in = surface_->points [point_idx];
246 PointOutT& point_out = output [point_idx];
248 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_distances))
250 point_out = kernel_ (nn_indices, nn_distances);
254 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.
virtual PointOutT operator()(const std::vector< int > &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
uint32_t width
The point cloud width (if organized as an image-structure).
A 2D point structure representing Euclidean xy coordinates.
void resize(size_t n)
Resize the cloud.
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 methode 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.
PointOutT operator()(const std::vector< int > &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
uint32_t height
The point cloud height (if organized as an image-structure).