Autoware.Auto
spatial_hash_config.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_CONFIG_HPP_
21 #define GEOMETRY__SPATIAL_HASH_CONFIG_HPP_
22 
23 #include <common/types.hpp>
25 #include <geometry/common_2d.hpp>
26 
27 #include <algorithm>
28 #include <cmath>
29 #include <limits>
30 #include <stdexcept>
31 #include <utility>
32 
34 
38 
39 namespace autoware
40 {
41 namespace common
42 {
43 namespace geometry
44 {
46 namespace spatial_hash
47 {
49 using Index = std::size_t;
51 namespace details
52 {
59 struct GEOMETRY_PUBLIC Index3
60 {
64 }; // struct Index3
65 
66 using BinRange = std::pair<Index3, Index3>;
67 } // namespace details
68 
71 template<typename Derived>
72 class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<Derived>
73 {
74 public:
85  const float32_t min_x,
86  const float32_t max_x,
87  const float32_t min_y,
88  const float32_t max_y,
89  const float32_t min_z,
90  const float32_t max_z,
91  const float32_t radius,
92  const Index capacity)
93  : m_min_x{min_x},
94  m_min_y{min_y},
95  m_min_z{min_z},
96  m_max_x{max_x},
97  m_max_y{max_y},
98  m_max_z{max_z},
99  m_side_length{radius},
100  m_side_length2{radius * radius},
101  m_side_length_inv{1.0F / radius},
102  m_capacity{capacity},
103  m_max_x_idx{check_basis_direction(min_x, max_x)},
104  m_max_y_idx{check_basis_direction(min_y, max_y)},
105  m_max_z_idx{check_basis_direction(min_z, max_z)},
106  m_y_stride{m_max_x_idx + 1U},
107  m_z_stride{m_y_stride * (m_max_y_idx + 1U)}
108  {
109  if (radius <= 0.0F) {
110  throw std::domain_error("Error constructing SpatialHash: must have positive side length");
111  }
112 
113  if ((m_max_y_idx + m_y_stride) > std::numeric_limits<Index>::max() / 2U) {
114  // TODO(c.ho) properly check for multiplication overflow
115  throw std::domain_error("SpatialHash::Config: voxel index may overflow!");
116  }
117  // small fudging to prevent weird boundary effects
118  // (e.g (x=xmax, y) rolls index over to (x=0, y+1)
119  constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
120  //lint -e{1938} read only access is fine NOLINT
121  m_max_x -= FEPS;
122  m_max_y -= FEPS;
123  m_max_z -= FEPS;
124  if ((m_z_stride + m_max_z_idx) > std::numeric_limits<Index>::max() / 2U) {
125  // TODO(c.ho) properly check for multiplication overflow
126  throw std::domain_error("SpatialHash::Config: voxel index may overflow!");
127  }
128  // I don't know if this is even possible given other checks
129  if (std::numeric_limits<Index>::max() == m_max_z_idx) {
130  throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value");
131  }
132  }
133 
140  details::BinRange bin_range(const details::Index3 & ref, const float radius) const
141  {
142  // Compute distance in units of voxels
143  const Index iradius = static_cast<Index>(std::ceil(radius / m_side_length));
144  // Dumb ternary because potentially unsigned Index type
145  const Index xmin = (ref.x > iradius) ? (ref.x - iradius) : 0U;
146  const Index ymin = (ref.y > iradius) ? (ref.y - iradius) : 0U;
147  const Index zmin = (ref.z > iradius) ? (ref.z - iradius) : 0U;
148  // In 2D mode, m_max_z should be 0, same with ref.z
149  const Index xmax = std::min(ref.x + iradius, m_max_x_idx);
150  const Index ymax = std::min(ref.y + iradius, m_max_y_idx);
151  const Index zmax = std::min(ref.z + iradius, m_max_z_idx);
152  // return bottom-left portion of cube and top-right portion of cube
153  return {{xmin, ymin, zmin}, {xmax, ymax, zmax}};
154  }
155 
160  bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const
161  {
162  // TODO(c.ho) is there any way to make this neater without triple nested if?
163  bool8_t ret = true;
164  ++idx.x;
165  if (idx.x > range.second.x) {
166  idx.x = range.first.x;
167  ++idx.y;
168  if (idx.y > range.second.y) {
169  idx.y = range.first.y;
170  ++idx.z;
171  if (idx.z > range.second.z) {
172  ret = false;
173  }
174  }
175  }
176  return ret;
177  }
181  {
182  return m_capacity;
183  }
184 
187  {
188  return m_side_length2;
189  }
190 
192  // "Polymorphic" API
198  Index bin(const float32_t x, const float32_t y, const float32_t z) const
199  {
200  return this->impl().bin_(x, y, z);
201  }
210  const details::Index3 & ref,
211  const details::Index3 & query,
212  const float ref_distance2) const
213  {
214  return this->impl().valid(ref, query, ref_distance2);
215  }
221  details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const
222  {
223  return this->impl().index3_(x, y, z);
224  }
228  Index index(const details::Index3 & idx) const
229  {
230  return this->impl().index_(idx);
231  }
239  template<typename PointT>
241  const float32_t x,
242  const float32_t y,
243  const float32_t z,
244  const PointT & pt) const
245  {
246  return this->impl().distance_squared_(x, y, z, pt);
247  }
248 
249 protected:
253  Index x_index(const float32_t x) const
254  {
255  return static_cast<Index>(
256  std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv));
257  }
261  Index y_index(const float32_t y) const
262  {
263  return static_cast<Index>(
264  std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv));
265  }
269  Index z_index(const float32_t z) const
270  {
271  return static_cast<Index>(
272  std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv));
273  }
275  Index bin_impl(const Index xdx, const Index ydx) const
276  {
277  return xdx + (ydx * m_y_stride);
278  }
280  Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const
281  {
282  return bin_impl(xdx, ydx) + (zdx * m_z_stride);
283  }
284 
289  Index bin_impl(const float32_t x, const float32_t y) const
290  {
291  return bin_impl(x_index(x), y_index(y));
292  }
298  Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const
299  {
300  return bin_impl(x, y) + (z_index(z) * m_z_stride);
301  }
304  float32_t idx_distance(const Index ref_idx, const Index query_idx) const
305  {
307  const Index idist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx);
308  float32_t dist = static_cast<float32_t>(idist) - 1.0F;
309  return std::max(dist, 0.0F);
310  }
311 
313  float side_length2() const
314  {
315  return m_side_length2;
316  }
317 
318 private:
321  Index check_basis_direction(const float32_t min, const float32_t max) const
322  {
323  if (min >= max) {
324  throw std::domain_error("SpatialHash::Config: must have min < max");
325  }
326  // This family of checks is to ensure that you don't get weird casting effects due to huge
327  // floating point values
328  const float64_t dmax = static_cast<float64_t>(max);
329  const float64_t dmin = static_cast<float64_t>(min);
330  const float64_t width = (dmax - dmin) * static_cast<float64_t>(m_side_length_inv);
331  constexpr float64_t fltmax = static_cast<float64_t>(std::numeric_limits<float32_t>::max());
332  if (fltmax <= width) {
333  throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit");
334  }
335  return static_cast<Index>(width);
336  }
337  float32_t m_min_x;
338  float32_t m_min_y;
339  float32_t m_min_z;
340  float32_t m_max_x;
341  float32_t m_max_y;
342  float32_t m_max_z;
343  float32_t m_side_length;
344  float32_t m_side_length2;
345  float32_t m_side_length_inv;
346  Index m_capacity;
347  Index m_max_x_idx;
348  Index m_max_y_idx;
349  Index m_max_z_idx;
350  Index m_y_stride;
351  Index m_z_stride;
352 }; // class Config
353 
355 class GEOMETRY_PUBLIC Config2d : public Config<Config2d>
356 {
357 public:
365  Config2d(
366  const float32_t min_x,
367  const float32_t max_x,
368  const float32_t min_y,
369  const float32_t max_y,
370  const float32_t radius,
371  const Index capacity);
377  Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
385  bool valid(
386  const details::Index3 & ref,
387  const details::Index3 & query,
388  const float ref_distance2) const;
394  details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
398  Index index_(const details::Index3 & idx) const;
406  template<typename PointT>
408  const float32_t x,
409  const float32_t y,
410  const float32_t z,
411  const PointT & pt) const
412  {
413  (void)z;
414  const float32_t dx = x - point_adapter::x_(pt);
415  const float32_t dy = y - point_adapter::y_(pt);
416  return (dx * dx) + (dy * dy);
417  }
418 }; // class Config2d
419 
421 class GEOMETRY_PUBLIC Config3d : public Config<Config3d>
422 {
423 public:
433  Config3d(
434  const float32_t min_x,
435  const float32_t max_x,
436  const float32_t min_y,
437  const float32_t max_y,
438  const float32_t min_z,
439  const float32_t max_z,
440  const float32_t radius,
441  const Index capacity);
447  Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
455  bool valid(
456  const details::Index3 & ref,
457  const details::Index3 & query,
458  const float ref_distance2) const;
464  details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
468  Index index_(const details::Index3 & idx) const;
476  template<typename PointT>
478  const float32_t x,
479  const float32_t y,
480  const float32_t z,
481  const PointT & pt) const
482  {
483  const float32_t dx = x - point_adapter::x_(pt);
484  const float32_t dy = y - point_adapter::y_(pt);
485  const float32_t dz = z - point_adapter::z_(pt);
486  return (dx * dx) + (dy * dy) + (dz * dz);
487  }
488 }; // class Config3d
489 } // namespace spatial_hash
490 } // namespace geometry
491 } // namespace common
492 } // namespace autoware
493 
494 #endif // GEOMETRY__SPATIAL_HASH_CONFIG_HPP_
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
visibility_control.hpp
autoware::common::geometry::spatial_hash::Config::distance_squared
float32_t distance_squared(const float32_t x, const float32_t y, const float32_t z, const PointT &pt) const
Compute the squared distance between the two points.
Definition: spatial_hash_config.hpp:240
autoware::common::geometry::spatial_hash::Config2d
Configuration class for a 2d spatial hash.
Definition: spatial_hash_config.hpp:355
common_2d.hpp
This file includes common functionality for 2D geometry, such as dot products.
autoware::common::geometry::spatial_hash::Config::next_bin
bool8_t next_bin(const details::BinRange &range, details::Index3 &idx) const
Get next index within a given range.
Definition: spatial_hash_config.hpp:160
autoware::common::geometry::spatial_hash::details::Index3::z
Index z
Definition: spatial_hash_config.hpp:63
autoware::common::geometry::spatial_hash::Config::Config
Config(const float32_t min_x, const float32_t max_x, const float32_t min_y, const float32_t max_y, const float32_t min_z, const float32_t max_z, const float32_t radius, const Index capacity)
Constructor for spatial hash.
Definition: spatial_hash_config.hpp:84
autoware::common::geometry::spatial_hash::Config::idx_distance
float32_t idx_distance(const Index ref_idx, const Index query_idx) const
The distance between two indices as a float, where adjacent indices have zero distance (e....
Definition: spatial_hash_config.hpp:304
autoware::common::geometry::spatial_hash::Config::y_index
Index y_index(const float32_t y) const
Computes the index in the y basis direction.
Definition: spatial_hash_config.hpp:261
autoware::common::geometry::spatial_hash::Config::z_index
Index z_index(const float32_t z) const
Computes the index in the z basis direction.
Definition: spatial_hash_config.hpp:269
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware::common::geometry::spatial_hash::Config::get_capacity
Index get_capacity() const
Get the maximum capacity of the spatial hash.
Definition: spatial_hash_config.hpp:180
autoware::common::geometry::spatial_hash::details::Index3
Internal struct for packing three indices together.
Definition: spatial_hash_config.hpp:59
autoware::common::geometry::spatial_hash::Config::is_candidate_bin
bool is_candidate_bin(const details::Index3 &ref, const details::Index3 &query, const float ref_distance2) const
Compute whether the query bin and reference bin could possibly contain a pair of points such that the...
Definition: spatial_hash_config.hpp:209
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::common::geometry::spatial_hash::Config::bin_impl
Index bin_impl(const Index xdx, const Index ydx) const
Compose the provided index offsets.
Definition: spatial_hash_config.hpp:275
autoware::common::geometry::spatial_hash::Config::bin_impl
Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const
The index of a point given it's x, y and z values.
Definition: spatial_hash_config.hpp:298
autoware::common::geometry::spatial_hash::Config
The base class for the configuration object for the SpatialHash class.
Definition: spatial_hash_config.hpp:72
autoware::common::helper_functions::crtp
Definition: crtp.hpp:29
autoware::common::geometry::spatial_hash::Config::index
Index index(const details::Index3 &idx) const
Compute the composed single index given a decomposed index.
Definition: spatial_hash_config.hpp:228
autoware::common::geometry::spatial_hash::Config::side_length2
float side_length2() const
Get side length squared.
Definition: spatial_hash_config.hpp:313
x
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
autoware::common::geometry::spatial_hash::Config::bin
Index bin(const float32_t x, const float32_t y, const float32_t z) const
Compute the single index given a point.
Definition: spatial_hash_config.hpp:198
autoware::common::types::float32_t
float float32_t
Definition: types.hpp:45
autoware::common::geometry::spatial_hash::Config::radius2
float32_t radius2() const
Getter for the side length, equivalently the lookup radius.
Definition: spatial_hash_config.hpp:186
crtp.hpp
This file includes common helper functions.
autoware::common::geometry::spatial_hash::Config2d::distance_squared_
float32_t distance_squared_(const float32_t x, const float32_t y, const float32_t z, const PointT &pt) const
Compute the squared distance between the two points, 2d implementation.
Definition: spatial_hash_config.hpp:407
autoware::common::geometry::spatial_hash::Config3d::distance_squared_
float32_t distance_squared_(const float32_t x, const float32_t y, const float32_t z, const PointT &pt) const
Compute the squared distance between the two points, 3d implementation.
Definition: spatial_hash_config.hpp:477
autoware::common::geometry::spatial_hash::details::Index3::x
Index x
Definition: spatial_hash_config.hpp:61
autoware::common::geometry::spatial_hash::Config3d
Configuration class for a 3d spatial hash.
Definition: spatial_hash_config.hpp:421
autoware::common::geometry::spatial_hash::details::Index3::y
Index y
Definition: spatial_hash_config.hpp:62
autoware::common::geometry::point_adapter::y_
auto y_(const PointT &pt)
Gets the y value for a point.
Definition: common_2d.hpp:66
autoware::common::geometry::spatial_hash::Config::index3
details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const
Compute the decomposed index given a point.
Definition: spatial_hash_config.hpp:221
autoware::common::types::float64_t
double float64_t
Definition: types.hpp:47
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::Config::x_index
Index x_index(const float32_t x) const
Computes the index in the x basis direction.
Definition: spatial_hash_config.hpp:253
autoware::common::geometry::spatial_hash::Config::bin_range
details::BinRange bin_range(const details::Index3 &ref, const float radius) const
Given a reference index triple, compute the first and last bin.
Definition: spatial_hash_config.hpp:140
autoware::common::geometry::spatial_hash::Config::bin_impl
Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const
Compose the provided index offsets.
Definition: spatial_hash_config.hpp:280
autoware::common::geometry::spatial_hash::details::BinRange
std::pair< Index3, Index3 > BinRange
Definition: spatial_hash_config.hpp:66
autoware::common::geometry::spatial_hash::Config::bin_impl
Index bin_impl(const float32_t x, const float32_t y) const
The index offset of a point given it's x and y values.
Definition: spatial_hash_config.hpp:289
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