Point Cloud Library (PCL)  1.13.0-dev
farthest_point_sampling.hpp
1 /*
2  * SPDX-License-Identifier: BSD-3-Clause
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2020-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  */
9 
10 #pragma once
11 
12 #include <pcl/common/geometry.h>
13 #include <pcl/filters/farthest_point_sampling.h>
14 #include <pcl/point_cloud.h>
15 #include <pcl/point_types.h>
16 #include <algorithm>
17 #include <limits>
18 #include <random>
19 
20 template<typename PointT> void
22 {
23  const std::size_t size = input_->size();
24  //if requested number of point is equal to the point cloud size, copy original cloud
25  if (sample_size_ == size)
26  {
27  indices = *indices_;
28  removed_indices_->clear ();
29  return;
30  }
31  //check if requested number of points is greater than the point cloud size
32  if (sample_size_ > size)
33  {
34  PCL_THROW_EXCEPTION (BadArgumentException,
35  "Requested number of points is greater than point cloud size!");
36  }
37 
38  std::vector<float> distances_to_selected_points (size, std::numeric_limits<float>::max ());
39 
40  //set random seed
41  std::mt19937 random_gen(seed_);
42  std::uniform_int_distribution<index_t> dis(0, size -1);
43 
44  //pick the first point at random
45  index_t max_index = dis(random_gen);
46  distances_to_selected_points[max_index] = -1.0;
47  indices.push_back(max_index);
48 
49  for (std::size_t j = 1; j < sample_size_; ++j)
50  {
51  index_t next_max_index = 0;
52 
53  const PointT& max_index_point = (*input_)[max_index];
54  //recompute distances
55  for (std::size_t i = 0; i < size; ++i)
56  {
57  if (distances_to_selected_points[i] == -1.0)
58  continue;
59  distances_to_selected_points[i] = std::min(distances_to_selected_points[i], geometry::distance((*input_)[i], max_index_point));
60  if (distances_to_selected_points[i] > distances_to_selected_points[next_max_index])
61  next_max_index = i;
62  }
63 
64  //select farthest point based on previously calculated distances
65  //since distance is set to -1 for all selected elements,previously selected
66  //elements are guaranteed to not be selected
67  max_index = next_max_index;
68  distances_to_selected_points[max_index] = -1.0;
69  indices.push_back(max_index);
70  //set distance to -1 to ignore during max element search
71  }
72 
73  if (extract_removed_indices_)
74  {
75  for (std::size_t k = 0; k < distances_to_selected_points.size(); ++k)
76  {
77  if (distances_to_selected_points[k] != -1.0)
78  (*removed_indices_).push_back(k);
79  }
80  }
81 }
82 
83 #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.
Definition: exceptions.h:256
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)
Definition: geometry.h:60
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.