Branch data Line data Source code
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.
16 : : /// \file
17 : : /// \brief This file implements a spatial hash for efficient fixed-radius near neighbor queries in
18 : : /// 2D
19 : :
20 : : #ifndef GEOMETRY__SPATIAL_HASH_CONFIG_HPP_
21 : : #define GEOMETRY__SPATIAL_HASH_CONFIG_HPP_
22 : :
23 : : #include <common/types.hpp>
24 : : #include <geometry/visibility_control.hpp>
25 : : #include <geometry/common_2d.hpp>
26 : :
27 : : #include <algorithm>
28 : : #include <cmath>
29 : : #include <limits>
30 : : #include <stdexcept>
31 : : #include <utility>
32 : :
33 : : #include "helper_functions/crtp.hpp"
34 : :
35 : : using autoware::common::types::float64_t;
36 : : using autoware::common::types::float32_t;
37 : : using autoware::common::types::bool8_t;
38 : :
39 : : namespace autoware
40 : : {
41 : : namespace common
42 : : {
43 : : namespace geometry
44 : : {
45 : : /// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup
46 : : namespace spatial_hash
47 : : {
48 : : /// \brief Index type for identifying bins of the hash/lattice
49 : : using Index = std::size_t;
50 : : /// \brief Spatial hash functionality not intended to be used by an external user
51 : : namespace details
52 : : {
53 : : /// \brief Internal struct for packing three indices together
54 : : ///
55 : : /// The use of this struct publically is a violation of our coding standards, but I claim it's
56 : : /// fine because (a) it's details, (b) it is literally three unrelated members packaged together.
57 : : /// This type is needed for conceptual convenience so I don't have massive function parameter
58 : : /// lists
59 : : struct GEOMETRY_PUBLIC Index3
60 : : {
61 : : Index x;
62 : : Index y;
63 : : Index z;
64 : : }; // struct Index3
65 : :
66 : : using BinRange = std::pair<Index3, Index3>;
67 : : } // namespace details
68 : :
69 : : /// \brief The base class for the configuration object for the SpatialHash class
70 : : /// \tparam Derived The type of the derived class to support static polymorphism/CRTP
71 : : template<typename Derived>
72 : : class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<Derived>
73 : : {
74 : : public:
75 : : /// \brief Constructor for spatial hash
76 : : /// \param[in] min_x The minimum x value for the spatial hash
77 : : /// \param[in] max_x The maximum x value for the spatial hash
78 : : /// \param[in] min_y The minimum y value for the spatial hash
79 : : /// \param[in] max_y The maximum y value for the spatial hash
80 : : /// \param[in] min_z The minimum z value for the spatial hash
81 : : /// \param[in] max_z The maximum z value for the spatial hash
82 : : /// \param[in] radius The look up radius
83 : : /// \param[in] capacity The maximum number of points the spatial hash can store
84 : 25 : Config(
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 : 25 : m_side_length2{radius * radius},
101 : 25 : 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 : 19 : m_y_stride{m_max_x_idx + 1U},
107 : 25 : m_z_stride{m_y_stride * (m_max_y_idx + 1U)}
108 : : {
109 [ + + ]: 19 : if (radius <= 0.0F) {
110 [ + - ]: 1 : throw std::domain_error("Error constructing SpatialHash: must have positive side length");
111 : : }
112 : :
113 [ - + ]: 18 : if ((m_max_y_idx + m_y_stride) > std::numeric_limits<Index>::max() / 2U) {
114 : : // TODO(c.ho) properly check for multiplication overflow
115 [ # # ]: 0 : 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 : 18 : m_max_x -= FEPS;
122 : 18 : m_max_y -= FEPS;
123 : 18 : m_max_z -= FEPS;
124 [ - + ]: 18 : if ((m_z_stride + m_max_z_idx) > std::numeric_limits<Index>::max() / 2U) {
125 : : // TODO(c.ho) properly check for multiplication overflow
126 [ # # ]: 0 : 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 [ - + ]: 18 : if (std::numeric_limits<Index>::max() == m_max_z_idx) {
130 [ # # ]: 0 : throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value");
131 : : }
132 : 18 : }
133 : :
134 : : /// \brief Given a reference index triple, compute the first and last bin
135 : : /// \param[in] ref The reference index triple
136 : : /// \return A pair where the first element is an index triple of the first bin, and the second
137 : : /// element is an index triple for the last bin
138 : 3 : details::BinRange bin_range(const details::Index3 & ref) const
139 : : {
140 : : // Compute distance in units of voxels
141 : 3 : constexpr Index iradius = 1;
142 : : // Dumb ternary because potentially unsigned Index type
143 : 3 : const Index xmin = (ref.x > iradius) ? (ref.x - iradius) : 0U;
144 : 3 : const Index ymin = (ref.y > iradius) ? (ref.y - iradius) : 0U;
145 : 3 : const Index zmin = (ref.z > iradius) ? (ref.z - iradius) : 0U;
146 : : // In 2D mode, m_max_z should be 0, same with ref.z
147 [ # # ]: 3 : const Index xmax = std::min(ref.x + iradius, m_max_x_idx);
148 [ # # ]: 3 : const Index ymax = std::min(ref.y + iradius, m_max_y_idx);
149 [ # # ]: 3 : const Index zmax = std::min(ref.z + iradius, m_max_z_idx);
150 : : // return bottom-left portion of cube and top-right portion of cube
151 : 3 : return {{xmin, ymin, zmin}, {xmax, ymax, zmax}};
152 : : }
153 : : /// \brief Get next index within a given range
154 : : /// \return True if idx is valid and still in range
155 : : /// \param[in] range The max and min bin indices
156 : : /// \param[inout] idx The index to be incremented, updated even if a negative result occurs
157 : 37 : bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const
158 : : {
159 : : // TODO(c.ho) is there any way to make this neater without triple nested if?
160 : 37 : bool8_t ret = true;
161 : 37 : ++idx.x;
162 [ + + # # ]: 37 : if (idx.x > range.second.x) {
163 : 13 : idx.x = range.first.x;
164 : 13 : ++idx.y;
165 [ + + # # ]: 13 : if (idx.y > range.second.y) {
166 : 5 : idx.y = range.first.y;
167 : 5 : ++idx.z;
168 [ + + # # ]: 5 : if (idx.z > range.second.z) {
169 : 3 : ret = false;
170 : : }
171 : : }
172 : : }
173 : 37 : return ret;
174 : : }
175 : : /// \brief Get the maximum capacity of the spatial hash
176 : : /// \return The capacity
177 : 27 : Index get_capacity() const
178 : : {
179 : 27 : return m_capacity;
180 : : }
181 : :
182 : : /// \brief Getter for the side length, equivalently the lookup radius
183 : 182 : float32_t radius2() const
184 : : {
185 : 182 : return m_side_length2;
186 : : }
187 : :
188 : : ////////////////////////////////////////////////////////////////////////////////////////////
189 : : // "Polymorphic" API
190 : : /// \brief Compute the single index given a point
191 : : /// \param[in] x The x component of the point
192 : : /// \param[in] y The y component of the point
193 : : /// \param[in] z The z component of the point
194 : : /// \return The combined index of the bin for a given point
195 : 182 : Index bin(const float32_t x, const float32_t y, const float32_t z) const
196 : : {
197 : 182 : return this->impl().bin_(x, y, z);
198 : : }
199 : : /// \brief Compute whether the query bin and reference bin could possibly contain a pair of points
200 : : /// such that their distance is within a certain threshold
201 : : /// \param[in] ref The index triple of the bin containing the reference point
202 : : /// \param[in] query The index triple of the bin being queried
203 : : /// \return True if query and ref could possibly hold points within reference distance to one
204 : : /// another
205 : 37 : bool8_t is_candidate_bin(const details::Index3 & ref, const details::Index3 & query) const
206 : : {
207 : 37 : return this->impl().valid(ref, query);
208 : : }
209 : : /// \brief Compute the decomposed index given a point
210 : : /// \param[in] x The x component of the point
211 : : /// \param[in] y The y component of the point
212 : : /// \param[in] z The z component of the point
213 : : /// \return The decomposed index triple of the bin for the given point
214 : 3 : details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const
215 : : {
216 : 3 : return this->impl().index3_(x, y, z);
217 : : }
218 : : /// \brief Compute the composed single index given a decomposed index
219 : : /// \param[in] idx A decomposed index triple for a bin
220 : : /// \return The composed bin index
221 : 37 : Index index(const details::Index3 & idx) const
222 : : {
223 : 37 : return this->impl().index_(idx);
224 : : }
225 : : /// \brief Compute the squared distance between the two points
226 : : /// \tparam PointT A point type with float members x, y and z, or point adapters defined
227 : : /// \param[in] x The x component of the first point
228 : : /// \param[in] y The y component of the first point
229 : : /// \param[in] z The z component of the first point
230 : : /// \param[in] pt The other point being compared
231 : : /// \return The squared distance between the points (2d or 3d)
232 : : template<typename PointT>
233 : 182 : float32_t distance_squared(
234 : : const float32_t x,
235 : : const float32_t y,
236 : : const float32_t z,
237 : : const PointT & pt) const
238 : : {
239 : 182 : return this->impl().distance_squared_(x, y, z, pt);
240 : : }
241 : :
242 : : protected:
243 : : /// \brief Computes the index in the x basis direction
244 : : /// \param[in] x The x value of a point
245 : : /// \return The x offset of the index
246 : 1479 : Index x_index(const float32_t x) const
247 : : {
248 : : return static_cast<Index>(
249 [ + + ]: 1479 : std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv));
250 : : }
251 : : /// \brief Computes the index in the y basis direction
252 : : /// \param[in] y The y value of a point
253 : : /// \return The x offset of the index
254 : 1479 : Index y_index(const float32_t y) const
255 : : {
256 : : return static_cast<Index>(
257 [ + + ]: 1479 : std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv));
258 : : }
259 : : /// \brief Computes the index in the z basis direction
260 : : /// \param[in] z The z value of a point
261 : : /// \return The x offset of the index
262 : 161 : Index z_index(const float32_t z) const
263 : : {
264 : : return static_cast<Index>(
265 [ - + ]: 161 : std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv));
266 : : }
267 : : /// \brief Compose the provided index offsets
268 : : Index bin_impl(const Index xdx, const Index ydx) const
269 : : {
270 : 6689 : return xdx + (ydx * m_y_stride);
271 : : }
272 : : /// \brief Compose the provided index offsets
273 : : Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const
274 : : {
275 : 27 : return bin_impl(xdx, ydx) + (zdx * m_z_stride);
276 : : }
277 : :
278 : : /// \brief The index offset of a point given it's x and y values
279 : : /// \param[in] x The x value of a point
280 : : /// \param[in] y the y value of a point
281 : : /// \return The index of the point in the 2D case, or the offset within a z-slice of the hash
282 : 829 : Index bin_impl(const float32_t x, const float32_t y) const
283 : : {
284 : 829 : return bin_impl(x_index(x), y_index(y));
285 : : }
286 : : /// \brief The index of a point given it's x, y and z values
287 : : /// \param[in] x The x value of a point
288 : : /// \param[in] y the y value of a point
289 : : /// \param[in] z the z value of a point
290 : : /// \return The index of the bin for the specified point
291 : 160 : Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const
292 : : {
293 : 160 : return bin_impl(x, y) + (z_index(z) * m_z_stride);
294 : : }
295 : : /// \brief The distance between two indices as a float, where adjacent indices have zero
296 : : /// distance (e.g. dist(0, 1) = 0)
297 : : float32_t idx_distance(const Index ref_idx, const Index query_idx) const
298 : : {
299 : : /// Not using fabs because Index is (possibly) unsigned
300 [ + + + + : 11747 : const Index idist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx);
+ + + + +
+ ]
301 : 11747 : float32_t dist = static_cast<float32_t>(idist) - 1.0F;
302 [ + + + + : 13710 : return std::max(dist, 0.0F);
+ + + + +
+ + + + +
+ + ]
303 : : }
304 : :
305 : : private:
306 : : /// \brief Sanity check a range in a basis direction
307 : : /// \return The number of voxels in the given basis direction
308 : 69 : Index check_basis_direction(const float32_t min, const float32_t max) const
309 : : {
310 [ + + ]: 69 : if (min >= max) {
311 [ + - ]: 3 : throw std::domain_error("SpatialHash::Config: must have min < max");
312 : : }
313 : : // This family of checks is to ensure that you don't get weird casting effects due to huge
314 : : // floating point values
315 : 66 : const float64_t dmax = static_cast<float64_t>(max);
316 : 66 : const float64_t dmin = static_cast<float64_t>(min);
317 : 66 : const float64_t width = (dmax - dmin) * static_cast<float64_t>(m_side_length_inv);
318 : : constexpr float64_t fltmax = static_cast<float64_t>(std::numeric_limits<float32_t>::max());
319 [ + + ]: 66 : if (fltmax <= width) {
320 [ + - ]: 3 : throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit");
321 : : }
322 : 63 : return static_cast<Index>(width);
323 : : }
324 : : float32_t m_min_x;
325 : : float32_t m_min_y;
326 : : float32_t m_min_z;
327 : : float32_t m_max_x;
328 : : float32_t m_max_y;
329 : : float32_t m_max_z;
330 : : float32_t m_side_length;
331 : : float32_t m_side_length2;
332 : : float32_t m_side_length_inv;
333 : : Index m_capacity;
334 : : Index m_max_x_idx;
335 : : Index m_max_y_idx;
336 : : Index m_max_z_idx;
337 : : Index m_y_stride;
338 : : Index m_z_stride;
339 : : }; // class Config
340 : :
341 : : /// \brief Configuration class for a 2d spatial hash
342 : : class GEOMETRY_PUBLIC Config2d : public Config<Config2d>
343 : : {
344 : : public:
345 : : /// \brief Config constructor for 2D spatial hash
346 : : /// \param[in] min_x The minimum x value for the spatial hash
347 : : /// \param[in] max_x The maximum x value for the spatial hash
348 : : /// \param[in] min_y The minimum y value for the spatial hash
349 : : /// \param[in] max_y The maximum y value for the spatial hash
350 : : /// \param[in] radius The lookup distance
351 : : /// \param[in] capacity The maximum number of points the spatial hash can store
352 : : Config2d(
353 : : const float32_t min_x,
354 : : const float32_t max_x,
355 : : const float32_t min_y,
356 : : const float32_t max_y,
357 : : const float32_t radius,
358 : : const Index capacity);
359 : : /// \brief The index of a point given it's x, y and z values, 2d implementation
360 : : /// \param[in] x The x value of a point
361 : : /// \param[in] y the y value of a point
362 : : /// \param[in] z the z value of a point
363 : : /// \return The index of the bin for the specified point
364 : : Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
365 : : /// \brief Determine if a bin could possibly hold a point within a distance to any point in a
366 : : /// reference bin for the 2D case
367 : : /// \param[in] ref The decomposed index triple of the reference bin
368 : : /// \param[in] query The decomposed index triple of the bin being queried
369 : : /// \return True if the reference bin and query bin could possibly hold a point within the
370 : : /// reference distance
371 : : bool8_t valid(const details::Index3 & ref, const details::Index3 & query) const;
372 : : /// \brief Compute the decomposed index given a point, 2d implementation
373 : : /// \param[in] x The x component of the point
374 : : /// \param[in] y The y component of the point
375 : : /// \param[in] z The z component of the point
376 : : /// \return The decomposed index triple of the bin for the given point
377 : : details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
378 : : /// \brief Compute the composed single index given a decomposed index, 2d implementation
379 : : /// \param[in] idx A decomposed index triple for a bin
380 : : /// \return The composed bin index
381 : : Index index_(const details::Index3 & idx) const;
382 : : /// \brief Compute the squared distance between the two points, 2d implementation
383 : : /// \tparam PointT A point type with float members x, y and z, or point adapters defined
384 : : /// \param[in] x The x component of the first point
385 : : /// \param[in] y The y component of the first point
386 : : /// \param[in] z The z component of the first point
387 : : /// \param[in] pt The other point being compared
388 : : /// \return The squared distance between the points (2d)
389 : : template<typename PointT>
390 : 22 : float32_t distance_squared_(
391 : : const float32_t x,
392 : : const float32_t y,
393 : : const float32_t z,
394 : : const PointT & pt) const
395 : : {
396 : : (void)z;
397 : 22 : const float32_t dx = x - point_adapter::x_(pt);
398 : 22 : const float32_t dy = y - point_adapter::y_(pt);
399 [ # # ]: 22 : return (dx * dx) + (dy * dy);
400 : : }
401 : : }; // class Config2d
402 : :
403 : : /// \brief Configuration class for a 3d spatial hash
404 : : class GEOMETRY_PUBLIC Config3d : public Config<Config3d>
405 : : {
406 : : public:
407 : : /// \brief Config constructor for a 3d spatial hash
408 : : /// \param[in] min_x The minimum x value for the spatial hash
409 : : /// \param[in] max_x The maximum x value for the spatial hash
410 : : /// \param[in] min_y The minimum y value for the spatial hash
411 : : /// \param[in] max_y The maximum y value for the spatial hash
412 : : /// \param[in] min_z The minimum z value for the spatial hash
413 : : /// \param[in] max_z The maximum z value for the spatial hash
414 : : /// \param[in] radius The lookup distance
415 : : /// \param[in] capacity The maximum number of points the spatial hash can store
416 : : Config3d(
417 : : const float32_t min_x,
418 : : const float32_t max_x,
419 : : const float32_t min_y,
420 : : const float32_t max_y,
421 : : const float32_t min_z,
422 : : const float32_t max_z,
423 : : const float32_t radius,
424 : : const Index capacity);
425 : : /// \brief The index of a point given it's x, y and z values, 3d implementation
426 : : /// \param[in] x The x value of a point
427 : : /// \param[in] y the y value of a point
428 : : /// \param[in] z the z value of a point
429 : : /// \return The index of the bin for the specified point
430 : : Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
431 : : /// \brief Determine if a bin could possibly hold a point within a distance to any point in a
432 : : /// reference bin for the 3D case
433 : : /// \param[in] ref The decomposed index triple of the reference bin
434 : : /// \param[in] query The decomposed index triple of the bin being queried
435 : : /// \return True if the reference bin and query bin could possibly hold a point within the
436 : : /// reference distance
437 : : bool8_t valid(const details::Index3 & ref, const details::Index3 & query) const;
438 : : /// \brief Compute the decomposed index given a point, 3d implementation
439 : : /// \param[in] x The x component of the point
440 : : /// \param[in] y The y component of the point
441 : : /// \param[in] z The z component of the point
442 : : /// \return The decomposed index triple of the bin for the given point
443 : : details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
444 : : /// \brief Compute the composed single index given a decomposed index, 3d implementation
445 : : /// \param[in] idx A decomposed index triple for a bin
446 : : /// \return The composed bin index
447 : : Index index_(const details::Index3 & idx) const;
448 : : /// \brief Compute the squared distance between the two points, 3d implementation
449 : : /// \tparam PointT A point type with float members x, y and z, or point adapters defined
450 : : /// \param[in] x The x component of the first point
451 : : /// \param[in] y The y component of the first point
452 : : /// \param[in] z The z component of the first point
453 : : /// \param[in] pt The other point being compared
454 : : /// \return The squared distance between the points (3d)
455 : : template<typename PointT>
456 : 160 : float32_t distance_squared_(
457 : : const float32_t x,
458 : : const float32_t y,
459 : : const float32_t z,
460 : : const PointT & pt) const
461 : : {
462 : 160 : const float32_t dx = x - point_adapter::x_(pt);
463 : 160 : const float32_t dy = y - point_adapter::y_(pt);
464 : 160 : const float32_t dz = z - point_adapter::z_(pt);
465 [ # # ]: 160 : return (dx * dx) + (dy * dy) + (dz * dz);
466 : : }
467 : : }; // class Config3d
468 : : } // namespace spatial_hash
469 : : } // namespace geometry
470 : : } // namespace common
471 : : } // namespace autoware
472 : :
473 : : #endif // GEOMETRY__SPATIAL_HASH_CONFIG_HPP_
|
