Open3D (C++ API)  0.12.0
RadiusSearch.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2019 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26 
27 #pragma once
28 
29 #include <tbb/parallel_for.h>
30 
31 #include <mutex>
32 
33 #include "open3d/core/Atomic.h"
36 
37 namespace open3d {
38 namespace ml {
39 namespace impl {
40 
41 namespace {
42 
45 template <class T, class OUTPUT_ALLOCATOR, int METRIC>
46 void _RadiusSearchCPU(int64_t* query_neighbors_row_splits,
47  size_t num_points,
48  const T* const points,
49  size_t num_queries,
50  const T* const queries,
51  const T* const radii,
52  bool ignore_query_point,
53  bool return_distances,
54  bool normalize_distances,
55  OUTPUT_ALLOCATOR& output_allocator) {
56  using namespace open3d::utility;
57 
58  // return empty indices array if there are no points
59  if (num_points == 0 || num_queries == 0) {
60  std::fill(query_neighbors_row_splits,
61  query_neighbors_row_splits + num_queries + 1, 0);
62  int32_t* indices_ptr;
63  output_allocator.AllocIndices(&indices_ptr, 0);
64 
65  T* distances_ptr;
66  output_allocator.AllocDistances(&distances_ptr, 0);
67 
68  return;
69  }
70 
71  Adaptor<T> adaptor(num_points, points);
72 
74  typename SelectNanoflannAdaptor<METRIC, T>::Adaptor_t, Adaptor<T>,
75  3>
76  KDTree_t;
77 
78  KDTree_t index(3, adaptor);
79  index.buildIndex();
80 
81  // temporary storage for the result
82  struct Pair {
83  int32_t i, j;
84  };
85  std::vector<Pair> pairs;
86 
87  // do not sort search results
88  nanoflann::SearchParams search_params(32, 0, false);
89 
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];
92  };
93 
94  auto distance_fn = [](const T* const p1, const T* const p2) {
95  T dx = p1[0] - p2[0];
96  T dy = p1[1] - p2[1];
97  T dz = p1[2] - p2[2];
98  if (METRIC == L2) {
99  return dx * dx + dy * dy + dz * dz;
100  } else // L1
101  {
102  return std::abs(dx) + std::abs(dy) + std::abs(dz);
103  }
104  };
105 
106  std::mutex pairs_mutex; // protects write access to pairs
107  std::vector<uint32_t> neighbors_count(num_queries, 0);
108 
109  // compute nearest neighbors and store in pairs
110  tbb::parallel_for(
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;
115 
116  for (size_t i = r.begin(); i != r.end(); ++i) {
117  T radius;
118  if (METRIC == L2)
119  radius = radii[i] * radii[i];
120  else // L1
121  radius = radii[i];
122  index.radiusSearch(&queries[i * 3], radius, search_result,
123  search_params);
124 
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])) {
130  continue;
131  }
132  pairs_private.push_back(
133  Pair{int32_t(i), int32_t(idx_dist.first)});
134  ++num_neighbors;
135  }
136  neighbors_count[i] = num_neighbors;
137  }
138  {
139  std::lock_guard<std::mutex> lock(pairs_mutex);
140  pairs.insert(pairs.end(), pairs_private.begin(),
141  pairs_private.end());
142  }
143  });
144 
145  query_neighbors_row_splits[0] = 0;
146  InclusivePrefixSum(&neighbors_count[0],
147  &neighbors_count[neighbors_count.size()],
148  query_neighbors_row_splits + 1);
149 
150  int32_t* neighbors_indices_ptr;
151  output_allocator.AllocIndices(&neighbors_indices_ptr, pairs.size());
152  T* distances_ptr;
153  if (return_distances)
154  output_allocator.AllocDistances(&distances_ptr, pairs.size());
155  else
156  output_allocator.AllocDistances(&distances_ptr, 0);
157 
158  std::fill(neighbors_count.begin(), neighbors_count.end(), 0);
159 
160  // fill output index and distance arrays
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];
165 
166  int64_t idx =
167  query_neighbors_row_splits[pair.i] +
169  &neighbors_count[pair.i], 1);
170  neighbors_indices_ptr[idx] = pair.j;
171 
172  if (return_distances) {
173  T dist = distance_fn(&points[pair.j * 3],
174  &queries[pair.i * 3]);
175  if (normalize_distances) {
176  if (METRIC == L2)
177  dist /= radii[pair.i] * radii[pair.i];
178  else // L1
179  dist /= radii[pair.i];
180  }
181  distances_ptr[idx] = dist;
182  }
183  }
184  });
185 }
186 
187 } // namespace
188 
240 template <class T, class OUTPUT_ALLOCATOR>
241 void RadiusSearchCPU(int64_t* query_neighbors_row_splits,
242  size_t num_points,
243  const T* const points,
244  size_t num_queries,
245  const T* const queries,
246  const T* radii,
247  const Metric metric,
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, \
255  output_allocator
256 
257 #define CALL_TEMPLATE(METRIC) \
258  if (METRIC == metric) \
259  _RadiusSearchCPU<T, OUTPUT_ALLOCATOR, METRIC>(FN_PARAMETERS);
260 
261 #define CALL_TEMPLATE2 \
262  CALL_TEMPLATE(L1) \
263  CALL_TEMPLATE(L2)
264 
266 
267 #undef CALL_TEMPLATE
268 #undef CALL_TEMPLATE2
269 
270 #undef FN_PARAMETERS
271 }
272 
273 } // namespace impl
274 } // namespace ml
275 } // namespace open3d
void InclusivePrefixSum(const Tin *first, const Tin *last, Tout *out)
Definition: ParallelScan.h:67
Metric
Supported metrics.
Definition: NeighborSearchCommon.h:38
#define CALL_TEMPLATE2
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