40#ifndef PCL_FILTERS_BILATERAL_IMPL_H_
41#define PCL_FILTERS_BILATERAL_IMPL_H_
43#include <pcl/filters/bilateral.h>
44#include <pcl/search/organized.h>
45#include <pcl/search/kdtree.h>
48template <
typename Po
intT>
double
51 const std::vector<float> &distances)
56 for (std::size_t n_id = 0; n_id < indices.size (); ++n_id)
58 int id = indices[n_id];
60 double intensity_dist = std::abs ((*input_)[pid].intensity - (*input_)[
id].intensity);
63 double dist = std::sqrt (distances[n_id]);
64 double weight =
kernel (dist, sigma_s_) *
kernel (intensity_dist, sigma_r_);
67 BF += weight * (*input_)[id].intensity;
74template <
typename Po
intT>
void
80 PCL_ERROR (
"[pcl::BilateralFilter::applyFilter] Need a sigma_s value given before continuing.\n");
87 if (input_->isOrganized ())
93 tree_->setInputCloud (input_);
96 std::vector<float> k_distances;
102 for (
const auto& idx : (*indices_))
105 tree_->radiusSearch (idx, sigma_s_ * 2, k_indices, k_distances);
108 output[idx].intensity =
static_cast<float> (computePointWeight (idx, k_indices, k_distances));
112#define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter<T>;
void applyFilter(PointCloud &output) override
Filter the input data and store the results into output.
double computePointWeight(const int pid, const Indices &indices, const std::vector< float > &distances)
Compute the intensity average for a single point.
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 point clouds.
IndicesAllocator<> Indices
Type used for indices in PCL.