Autoware.Auto
spatial_hash.hpp
Go to the documentation of this file.
1 // Copyright 2019 the Autoware Foundation
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 //    http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 //
15 // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
19 
20 #ifndef GEOMETRY__SPATIAL_HASH_HPP_
21 #define GEOMETRY__SPATIAL_HASH_HPP_
22 
23 #include <common/types.hpp>
26 #include <vector>
27 #include <unordered_map>
28 #include <utility>
29 
32 
33 namespace autoware
34 {
35 namespace common
36 {
37 namespace geometry
38 {
40 namespace spatial_hash
41 {
42 
50 template<typename PointT, typename ConfigT>
51 class GEOMETRY_PUBLIC SpatialHashBase
52 {
53  using Index3 = details::Index3;
54  //lint -e{9131} NOLINT There's no other way to make this work in a static assert
55  static_assert(
56  std::is_same<ConfigT, Config2d>::value || std::is_same<ConfigT, Config3d>::value,
57  "SpatialHash only works with Config2d or Config3d");
58 
59 public:
60  using Hash = std::unordered_multimap<Index, PointT>;
61  using IT = typename Hash::const_iterator;
63  class Output
64  {
65 public:
69  Output(const IT iterator, const float32_t distance)
70  : m_iterator(iterator),
71  m_distance(distance)
72  {
73  }
76  const PointT & get_point() const
77  {
78  return m_iterator->second;
79  }
82  IT get_iterator() const
83  {
84  return m_iterator;
85  }
88  operator const PointT &() const
89  {
90  return get_point();
91  }
94  operator IT() const
95  {
96  return get_iterator();
97  }
101  {
102  return m_distance;
103  }
104 
105 private:
106  IT m_iterator;
107  float32_t m_distance;
108  }; // class Output
109  using OutputVector = typename std::vector<Output>;
110 
113  explicit SpatialHashBase(const ConfigT & cfg)
114  : m_config{cfg},
115  m_hash(),
116  m_neighbors{}, // TODO(c.ho) reserve, but there's no default constructor for output
117  m_bins_hit{}, // zero initialization (and below)
118  m_neighbors_found{}
119  {
120  }
121 
126  IT insert(const PointT & pt)
127  {
128  if (size() >= capacity()) {
129  throw std::length_error{"SpatialHash: Cannot insert past capacity"};
130  }
131  return insert_impl(pt);
132  }
133 
140  template<typename IteratorT>
141  void insert(IteratorT begin, IteratorT end)
142  {
143  // This check is here for strong exception safety
144  if ((size() + std::distance(begin, end)) > capacity()) {
145  throw std::length_error{"SpatialHash: Cannot multi-insert past capacity"};
146  }
147  for (IteratorT it = begin; it != end; ++it) {
148  (void)insert_impl(*it);
149  }
150  }
151 
160  IT erase(const IT point)
161  {
162  if (end() == m_hash.find(point->first)) {
163  throw std::domain_error{"SpatialHash: Attempting to erase invalid iterator"};
164  }
165  return m_hash.erase(point);
166  }
167 
169  void clear()
170  {
171  m_hash.clear();
172  }
175  Index size() const
176  {
177  return m_hash.size();
178  }
181  Index capacity() const
182  {
183  return m_config.get_capacity();
184  }
187  bool8_t empty() const
188  {
189  return m_hash.empty();
190  }
193  IT begin() const
194  {
195  return m_hash.begin();
196  }
199  IT end() const
200  {
201  return m_hash.end();
202  }
205  IT cbegin() const
206  {
207  return begin();
208  }
211  IT cend() const
212  {
213  return end();
214  }
215 
219  Index bins_hit() const
220  {
221  return m_bins_hit;
222  }
223 
228  {
229  return m_neighbors_found;
230  }
231 
232 protected:
242  const float32_t x,
243  const float32_t y,
244  const float32_t z,
245  const float32_t radius)
246  {
247  // reset output
248  m_neighbors.clear();
249  // Compute bin, bin range
250  const Index3 ref_idx = m_config.index3(x, y, z);
251  const float32_t radius2 = radius * radius;
252  const details::BinRange idx_range = m_config.bin_range(ref_idx, radius);
253  Index3 idx = idx_range.first;
254  // For bins in radius
255  do { // guaranteed to have at least the bin ref_idx is in
256  // update book-keeping
257  ++m_bins_hit;
258  // Iterating in a square/cube pattern is easier than constructing sphere pattern
259  if (m_config.is_candidate_bin(ref_idx, idx, radius2)) {
260  // For point in bin
261  const Index jdx = m_config.index(idx);
262  const auto range = m_hash.equal_range(jdx);
263  for (auto it = range.first; it != range.second; ++it) {
264  const auto & pt = it->second;
265  const float32_t dist2 = m_config.distance_squared(x, y, z, pt);
266  if (dist2 <= radius2) {
267  // Only compute true distance if necessary
268  m_neighbors.emplace_back(it, sqrtf(dist2));
269  }
270  }
271  }
272  } while (m_config.next_bin(idx_range, idx));
273  // update book-keeping
274  m_neighbors_found += m_neighbors.size();
275  return m_neighbors;
276  }
277 
278 private:
281  GEOMETRY_LOCAL IT insert_impl(const PointT & pt)
282  {
283  // Compute bin
284  const Index idx =
285  m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt));
286  // Insert into bin
287  return m_hash.insert(std::make_pair(idx, pt));
288  }
289 
290  const ConfigT m_config;
291  Hash m_hash;
292  OutputVector m_neighbors;
293  Index m_bins_hit;
294  Index m_neighbors_found;
295 }; // class SpatialHashBase
296 
301 template<typename PointT, typename ConfigT>
302 class GEOMETRY_PUBLIC SpatialHash;
303 
306 template<typename PointT>
307 class GEOMETRY_PUBLIC SpatialHash<PointT, Config2d>: public SpatialHashBase<PointT, Config2d>
308 {
309 public:
311 
312  explicit SpatialHash(const Config2d & cfg)
313  : SpatialHashBase<PointT, Config2d>(cfg) {}
314 
322  const float32_t x,
323  const float32_t y,
324  const float32_t radius)
325  {
326  return this->near_impl(x, y, 0.0F, radius);
327  }
328 
334  const OutputVector & near(const PointT & pt, const float32_t radius)
335  {
336  return near(point_adapter::x_(pt), point_adapter::y_(pt), radius);
337  }
338 };
339 
342 template<typename PointT>
343 class GEOMETRY_PUBLIC SpatialHash<PointT, Config3d>: public SpatialHashBase<PointT, Config3d>
344 {
345 public:
347 
348  explicit SpatialHash(const Config3d & cfg)
349  : SpatialHashBase<PointT, Config3d>(cfg) {}
350 
360  const float32_t x,
361  const float32_t y,
362  const float32_t z,
363  const float32_t radius)
364  {
365  return this->near_impl(x, y, z, radius);
366  }
367 
373  const OutputVector & near(const PointT & pt, const float32_t radius)
374  {
375  return near(
377  radius);
378  }
379 };
380 
381 template<typename T>
382 using SpatialHash2d = SpatialHash<T, Config2d>;
383 template<typename T>
384 using SpatialHash3d = SpatialHash<T, Config3d>;
385 } // namespace spatial_hash
386 } // namespace geometry
387 } // namespace common
388 } // namespace autoware
389 
390 #endif // GEOMETRY__SPATIAL_HASH_HPP_
autoware::common::geometry::spatial_hash::SpatialHashBase::Output
Wrapper around an iterator and a distance (from some query point)
Definition: spatial_hash.hpp:63
autoware::common::geometry::spatial_hash::SpatialHashBase::cend
IT cend() const
Get iterator to end of data structure.
Definition: spatial_hash.hpp:211
autoware::common::geometry::spatial_hash::SpatialHashBase::begin
IT begin() const
Get iterator to beginning of data structure.
Definition: spatial_hash.hpp:193
autoware::common::geometry::spatial_hash::SpatialHashBase< PointT, Config2d >::Hash
std::unordered_multimap< Index, PointT > Hash
Definition: spatial_hash.hpp:60
autoware::common::geometry::point_adapter::x_
auto x_(const PointT &pt)
Gets the x value for a point.
Definition: common_2d.hpp:50
types.hpp
This file includes common type definition.
y
DifferentialState y
Definition: kinematic_bicycle.snippet.hpp:28
autoware::common::geometry::spatial_hash::SpatialHashBase::size
Index size() const
Get current number of element stored in this data structure.
Definition: spatial_hash.hpp:175
visibility_control.hpp
autoware::common::geometry::spatial_hash::SpatialHashBase::near_impl
const OutputVector & near_impl(const float32_t x, const float32_t y, const float32_t z, const float32_t radius)
Finds all points within a fixed radius of a reference point.
Definition: spatial_hash.hpp:241
autoware::common::geometry::spatial_hash::SpatialHashBase< PointT, Config2d >::IT
typename Hash::const_iterator IT
Definition: spatial_hash.hpp:61
autoware::common::geometry::spatial_hash::SpatialHashBase< PointT, Config2d >::OutputVector
typename std::vector< Output > OutputVector
Definition: spatial_hash.hpp:109
autoware::common::geometry::spatial_hash::SpatialHashBase::cbegin
IT cbegin() const
Get iterator to beginning of data structure.
Definition: spatial_hash.hpp:205
autoware::common::geometry::spatial_hash::Config2d
Configuration class for a 2d spatial hash.
Definition: spatial_hash_config.hpp:355
autoware::common::geometry::spatial_hash::SpatialHash< PointT, Config2d >::SpatialHash
SpatialHash(const Config2d &cfg)
Definition: spatial_hash.hpp:312
autoware::common::geometry::spatial_hash::SpatialHash< PointT, Config2d >::near
const OutputVector & near(const PointT &pt, const float32_t radius)
Finds all points within a fixed radius of a reference point.
Definition: spatial_hash.hpp:334
autoware::common::geometry::spatial_hash::SpatialHashBase::Output::get_distance
float32_t get_distance() const
Get distance to reference point.
Definition: spatial_hash.hpp:100
autoware::common::geometry::spatial_hash::SpatialHashBase::capacity
Index capacity() const
Get the maximum capacity of the data structure.
Definition: spatial_hash.hpp:181
autoware::common::geometry::spatial_hash::SpatialHashBase::Output::get_iterator
IT get_iterator() const
Get underlying iterator.
Definition: spatial_hash.hpp:82
autoware::common::geometry::spatial_hash::SpatialHashBase::Output::Output
Output(const IT iterator, const float32_t distance)
Constructor.
Definition: spatial_hash.hpp:69
autoware::common::geometry::spatial_hash::SpatialHashBase::insert
IT insert(const PointT &pt)
Inserts point.
Definition: spatial_hash.hpp:126
autoware::common::geometry::spatial_hash::SpatialHash< PointT, Config3d >::near
const OutputVector & near(const float32_t x, const float32_t y, const float32_t z, const float32_t radius)
Finds all points within a fixed radius of a reference point.
Definition: spatial_hash.hpp:359
autoware::common::geometry::spatial_hash::SpatialHash< PointT, Config3d >::near
const OutputVector & near(const PointT &pt, const float32_t radius)
Finds all points within a fixed radius of a reference point.
Definition: spatial_hash.hpp:373
autoware::common::geometry::spatial_hash::SpatialHashBase::clear
void clear()
Reset the state of the data structure.
Definition: spatial_hash.hpp:169
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware::common::geometry::spatial_hash::SpatialHash< PointT, Config2d >::OutputVector
typename SpatialHashBase< PointT, Config2d >::OutputVector OutputVector
Definition: spatial_hash.hpp:310
autoware::common::geometry::spatial_hash::SpatialHash< PointT, Config2d >::near
const OutputVector & near(const float32_t x, const float32_t y, const float32_t radius)
Finds all points within a fixed radius of a reference point.
Definition: spatial_hash.hpp:321
autoware::perception::segmentation::euclidean_cluster::Hash
autoware::common::geometry::spatial_hash::SpatialHash2d< PointXYZIR > Hash
Definition: euclidean_cluster.hpp:88
autoware::common::geometry::spatial_hash::details::Index3
Internal struct for packing three indices together.
Definition: spatial_hash_config.hpp:59
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::common::geometry::spatial_hash::SpatialHash< PointT, Config3d >::OutputVector
typename SpatialHashBase< PointT, Config3d >::OutputVector OutputVector
Definition: spatial_hash.hpp:346
autoware::common::geometry::spatial_hash::SpatialHashBase::bins_hit
Index bins_hit() const
Get the number of bins touched during the lifetime of this object, for debugging and size tuning.
Definition: spatial_hash.hpp:219
autoware::common::geometry::spatial_hash::SpatialHash
class GEOMETRY_PUBLIC SpatialHash
The class to be used for specializing on apex_app::common::geometry::spatial_hash::SpatialHashBase to...
Definition: spatial_hash.hpp:302
x
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
autoware::common::geometry::spatial_hash::SpatialHashBase::neighbors_found
Index neighbors_found() const
Get number of near neighbors found during the lifetime of this object, for debugging and size tuning.
Definition: spatial_hash.hpp:227
autoware::common::geometry::spatial_hash::SpatialHashBase::insert
void insert(IteratorT begin, IteratorT end)
Inserts a range of points.
Definition: spatial_hash.hpp:141
autoware::common::geometry::spatial_hash::SpatialHashBase
An implementation of the spatial hash or integer lattice data structure for efficient (O(1)) near nei...
Definition: spatial_hash.hpp:51
autoware::common::geometry::spatial_hash::SpatialHashBase::empty
bool8_t empty() const
Whether the hash is empty.
Definition: spatial_hash.hpp:187
autoware::common::geometry::spatial_hash::SpatialHashBase::SpatialHashBase
SpatialHashBase(const ConfigT &cfg)
Constructor.
Definition: spatial_hash.hpp:113
autoware::common::types::float32_t
float float32_t
Definition: types.hpp:45
autoware::common::geometry::spatial_hash::SpatialHash3d
SpatialHash< T, Config3d > SpatialHash3d
Definition: spatial_hash.hpp:384
autoware::common::geometry::spatial_hash::SpatialHashBase::erase
IT erase(const IT point)
Removes the specified element from the data structure.
Definition: spatial_hash.hpp:160
autoware::common::geometry::spatial_hash::SpatialHash< PointT, Config3d >::SpatialHash
SpatialHash(const Config3d &cfg)
Definition: spatial_hash.hpp:348
autoware::common::geometry::spatial_hash::SpatialHash2d
SpatialHash< T, Config2d > SpatialHash2d
Definition: spatial_hash.hpp:382
autoware::common::geometry::spatial_hash::Config3d
Configuration class for a 3d spatial hash.
Definition: spatial_hash_config.hpp:421
autoware::common::geometry::point_adapter::y_
auto y_(const PointT &pt)
Gets the y value for a point.
Definition: common_2d.hpp:66
spatial_hash_config.hpp
This file implements a spatial hash for efficient fixed-radius near neighbor queries in 2D.
autoware::common::geometry::point_adapter::z_
auto z_(const PointT &pt)
Gets the z value for a point.
Definition: common_2d.hpp:82
autoware::common::geometry::spatial_hash::details::BinRange
std::pair< Index3, Index3 > BinRange
Definition: spatial_hash_config.hpp:66
get_open_port.end
end
Definition: scripts/get_open_port.py:23
autoware::common::geometry::spatial_hash::SpatialHashBase::Output::get_point
const PointT & get_point() const
Get stored point.
Definition: spatial_hash.hpp:76
autoware::common::geometry::spatial_hash::SpatialHashBase::end
IT end() const
Get iterator to end of data structure.
Definition: spatial_hash.hpp:199
autoware::common::geometry::spatial_hash::Index
std::size_t Index
Index type for identifying bins of the hash/lattice.
Definition: spatial_hash_config.hpp:49