13 #include <pcl/filters/farthest_point_sampling.h>
14 #include <pcl/point_cloud.h>
20 template<
typename Po
intT>
void
23 const std::size_t size = indices_->size();
25 if (sample_size_ == size)
28 removed_indices_->clear ();
32 if (sample_size_ > size)
35 "Requested number of points is greater than point cloud size!");
38 std::vector<float> distances_to_selected_points (size, std::numeric_limits<float>::max ());
41 std::mt19937 random_gen(seed_);
42 std::uniform_int_distribution<index_t> dis(0, size -1);
45 auto toCloudIndex = [
this](
const auto idx){
return (*indices_)[idx];};
48 index_t max_index = dis(random_gen);
49 distances_to_selected_points[max_index] = -1.0;
50 indices.push_back(toCloudIndex(max_index));
52 for (std::size_t j = 1; j < sample_size_; ++j)
54 const PointT& max_index_point = (*input_)[toCloudIndex(max_index)];
56 #pragma omp parallel \
58 shared(distances_to_selected_points, max_index, max_index_point, size, toCloudIndex) \
59 num_threads(num_threads_)
61 std::ptrdiff_t local_max_index = max_index;
64 for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(size); ++i)
66 if (distances_to_selected_points[i] == -1.0)
68 distances_to_selected_points[i] = std::min(distances_to_selected_points[i],
geometry::distance((*input_)[toCloudIndex(i)], max_index_point));
69 if (distances_to_selected_points[i] > distances_to_selected_points[local_max_index]) {
76 if (distances_to_selected_points[local_max_index] > distances_to_selected_points[max_index])
77 max_index = local_max_index;
84 distances_to_selected_points[max_index] = -1.0;
85 indices.push_back(toCloudIndex(max_index));
89 if (extract_removed_indices_)
91 for (std::size_t k = 0; k < distances_to_selected_points.size(); ++k)
93 if (distances_to_selected_points[k] != -1.0)
94 (*removed_indices_).push_back(toCloudIndex(k));
99 #define PCL_INSTANTIATE_FarthestPointSampling(T) template class PCL_EXPORTS pcl::FarthestPointSampling<T>;
An exception that is thrown when the arguments number or type is wrong/unhandled.
void applyFilter(pcl::Indices &indices) override
Sample of point indices.
Defines some geometrical functions and utility functions.
Defines all the PCL implemented PointT point type structures.
float distance(const PointT &p1, const PointT &p2)
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.