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)
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 ();
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;
129 template<
typename Po
intInT,
typename Po
intOutT> PointOutT
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 ();
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);
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 ())
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!");
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");
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];
250 point_out =
kernel_ (nn_indices, nn_distances);
254 kernel_.makeInfinite (point_out);
A point structure representing normal coordinates and the surface curvature estimate.
KdTreePtr tree_
A pointer to the spatial search object.
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.
PointCloudInConstPtr surface_
An input point cloud describing the surface that is to be used for nearest neighbors estimation...
virtual PointOutT operator()(const std::vector< int > &indices, const std::vector< float > &distances)
Convolve point at the center of this local information.
KernelT kernel_
convlving kernel
A 2D point structure representing Euclidean xy coordinates.
uint32_t height
The point cloud height (if organized as an image-structure).
PointCloudInConstPtr input_
source cloud
uint32_t width
The point cloud width (if organized as an image-structure).
double search_radius_
The nearest neighbors search radius for each point.
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
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...
PointCloudConstPtr input_
The input point cloud dataset.
void resize(size_t n)
Resize the cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.