Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
20template<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.
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.