29 #include <tbb/parallel_for.h> 45 template <
class T,
class OUTPUT_ALLOCATOR,
int METRIC>
46 void _RadiusSearchCPU(int64_t* query_neighbors_row_splits,
50 const T*
const queries,
52 bool ignore_query_point,
53 bool return_distances,
54 bool normalize_distances,
55 OUTPUT_ALLOCATOR& output_allocator) {
59 if (num_points == 0 || num_queries == 0) {
60 std::fill(query_neighbors_row_splits,
61 query_neighbors_row_splits + num_queries + 1, 0);
63 output_allocator.AllocIndices(&indices_ptr, 0);
66 output_allocator.AllocDistances(&distances_ptr, 0);
71 Adaptor<T> adaptor(num_points, points);
74 typename SelectNanoflannAdaptor<METRIC, T>::Adaptor_t, Adaptor<T>,
78 KDTree_t index(3, adaptor);
85 std::vector<Pair> pairs;
88 nanoflann::SearchParams search_params(32, 0,
false);
90 auto points_equal = [](
const T*
const p1,
const T*
const p2) {
91 return p1[0] == p2[0] && p1[1] == p2[1] && p1[2] == p2[2];
94 auto distance_fn = [](
const T*
const p1,
const T*
const p2) {
99 return dx * dx + dy * dy + dz * dz;
102 return std::abs(dx) + std::abs(dy) + std::abs(dz);
106 std::mutex pairs_mutex;
107 std::vector<uint32_t> neighbors_count(num_queries, 0);
111 tbb::blocked_range<size_t>(0, num_queries),
112 [&](
const tbb::blocked_range<size_t>& r) {
113 std::vector<Pair> pairs_private;
114 std::vector<std::pair<size_t, T>> search_result;
116 for (
size_t i = r.begin(); i != r.end(); ++i) {
119 radius = radii[i] * radii[i];
122 index.radiusSearch(&queries[i * 3], radius, search_result,
125 int num_neighbors = 0;
126 for (
const auto& idx_dist : search_result) {
127 if (ignore_query_point &&
128 points_equal(&queries[i * 3],
129 &points[idx_dist.first * 3])) {
132 pairs_private.push_back(
136 neighbors_count[i] = num_neighbors;
139 std::lock_guard<std::mutex> lock(pairs_mutex);
140 pairs.insert(pairs.end(), pairs_private.begin(),
141 pairs_private.end());
145 query_neighbors_row_splits[0] = 0;
147 &neighbors_count[neighbors_count.size()],
148 query_neighbors_row_splits + 1);
150 int32_t* neighbors_indices_ptr;
151 output_allocator.AllocIndices(&neighbors_indices_ptr, pairs.size());
153 if (return_distances)
154 output_allocator.AllocDistances(&distances_ptr, pairs.size());
156 output_allocator.AllocDistances(&distances_ptr, 0);
158 std::fill(neighbors_count.begin(), neighbors_count.end(), 0);
161 tbb::parallel_for(tbb::blocked_range<size_t>(0, pairs.size()),
162 [&](
const tbb::blocked_range<size_t>& r) {
163 for (
size_t i = r.begin(); i != r.end(); ++i) {
164 Pair pair = pairs[i];
167 query_neighbors_row_splits[pair.i] +
169 &neighbors_count[pair.i], 1);
170 neighbors_indices_ptr[idx] = pair.j;
172 if (return_distances) {
173 T dist = distance_fn(&points[pair.j * 3],
174 &queries[pair.i * 3]);
175 if (normalize_distances) {
177 dist /= radii[pair.i] * radii[pair.i];
179 dist /= radii[pair.i];
181 distances_ptr[idx] = dist;
240 template <
class T,
class OUTPUT_ALLOCATOR>
243 const T*
const points,
245 const T*
const queries,
248 const bool ignore_query_point,
249 const bool return_distances,
250 const bool normalize_distances,
251 OUTPUT_ALLOCATOR& output_allocator) {
252 #define FN_PARAMETERS \ 253 query_neighbors_row_splits, num_points, points, num_queries, queries, \ 254 radii, ignore_query_point, return_distances, normalize_distances, \ 257 #define CALL_TEMPLATE(METRIC) \ 258 if (METRIC == metric) \ 259 _RadiusSearchCPU<T, OUTPUT_ALLOCATOR, METRIC>(FN_PARAMETERS); 261 #define CALL_TEMPLATE2 \ 268 #undef CALL_TEMPLATE2
void InclusivePrefixSum(const Tin *first, const Tin *last, Tout *out)
Definition: ParallelScan.h:67
Metric
Supported metrics.
Definition: NeighborSearchCommon.h:38
const char const char value recording_handle imu_sample recording_handle uint8_t size_t data_size k4a_record_configuration_t config target_format k4a_capture_t capture_handle k4a_imu_sample_t imu_sample playback_handle k4a_logging_message_cb_t void min_level device_handle k4a_imu_sample_t int32_t
Definition: K4aPlugin.cpp:398
void RadiusSearchCPU(int64_t *query_neighbors_row_splits, size_t num_points, const T *const points, size_t num_queries, const T *const queries, const T *radii, const Metric metric, const bool ignore_query_point, const bool return_distances, const bool normalize_distances, OUTPUT_ALLOCATOR &output_allocator)
Definition: RadiusSearch.h:241
int points
Definition: FilePCD.cpp:73
Definition: NanoFlannIndex.h:45
Definition: NeighborSearchCommon.h:38
Definition: PinholeCameraIntrinsic.cpp:35
uint32_t AtomicFetchAddRelaxed(uint32_t *address, uint32_t val)
Definition: Atomic.h:40
Definition: Console.cpp:51