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 : 18 : 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 : : /// \param[in] radius The allowable radius for any point in the reference bin to any point in the
137 : : /// range
138 : : /// \return A pair where the first element is an index triple of the first bin, and the second
139 : : /// element is an index triple for the last bin
140 : 4 : details::BinRange bin_range(const details::Index3 & ref, const float radius) const
141 : : {
142 : : // Compute distance in units of voxels
143 : 4 : const Index iradius = static_cast<Index>(std::ceil(radius / m_side_length));
144 : : // Dumb ternary because potentially unsigned Index type
145 [ + + ]: 4 : const Index xmin = (ref.x > iradius) ? (ref.x - iradius) : 0U;
146 [ + + ]: 4 : const Index ymin = (ref.y > iradius) ? (ref.y - iradius) : 0U;
147 [ - + ]: 4 : 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 : 4 : const Index xmax = std::min(ref.x + iradius, m_max_x_idx);
150 : 4 : const Index ymax = std::min(ref.y + iradius, m_max_y_idx);
151 : 4 : 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 : 4 : return {{xmin, ymin, zmin}, {xmax, ymax, zmax}};
154 : : }
155 : :
156 : : /// \brief Get next index within a given range
157 : : /// \return True if idx is valid and still in range
158 : : /// \param[in] range The max and min bin indices
159 : : /// \param[inout] idx The index to be incremented, updated even if a negative result occurs
160 : 46 : 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 : 46 : bool8_t ret = true;
164 : 46 : ++idx.x;
165 [ + + ]: 46 : if (idx.x > range.second.x) {
166 : 16 : idx.x = range.first.x;
167 : 16 : ++idx.y;
168 [ + + ]: 16 : if (idx.y > range.second.y) {
169 : 6 : idx.y = range.first.y;
170 : 6 : ++idx.z;
171 [ + + ]: 6 : if (idx.z > range.second.z) {
172 : 4 : ret = false;
173 : : }
174 : : }
175 : : }
176 : 46 : return ret;
177 : : }
178 : : /// \brief Get the maximum capacity of the spatial hash
179 : : /// \return The capacity
180 : 27 : Index get_capacity() const
181 : : {
182 : 27 : return m_capacity;
183 : : }
184 : :
185 : : /// \brief Getter for the side length, equivalently the lookup radius
186 : : float32_t radius2() const
187 : : {
188 : : return m_side_length2;
189 : : }
190 : :
191 : : ////////////////////////////////////////////////////////////////////////////////////////////
192 : : // "Polymorphic" API
193 : : /// \brief Compute the single index given a point
194 : : /// \param[in] x The x component of the point
195 : : /// \param[in] y The y component of the point
196 : : /// \param[in] z The z component of the point
197 : : /// \return The combined index of the bin for a given point
198 : 182 : Index bin(const float32_t x, const float32_t y, const float32_t z) const
199 : : {
200 : 182 : return this->impl().bin_(x, y, z);
201 : : }
202 : : /// \brief Compute whether the query bin and reference bin could possibly contain a pair of points
203 : : /// such that their distance is within a certain threshold
204 : : /// \param[in] ref The index triple of the bin containing the reference point
205 : : /// \param[in] query The index triple of the bin being queried
206 : : /// \param[in] ref_distance2 The squared threshold distance
207 : : /// \return True if query and ref could possibly hold points within reference distance to one
208 : : /// another
209 : 46 : bool is_candidate_bin(
210 : : const details::Index3 & ref,
211 : : const details::Index3 & query,
212 : : const float ref_distance2) const
213 : : {
214 : 46 : return this->impl().valid(ref, query, ref_distance2);
215 : : }
216 : : /// \brief Compute the decomposed index given a point
217 : : /// \param[in] x The x component of the point
218 : : /// \param[in] y The y component of the point
219 : : /// \param[in] z The z component of the point
220 : : /// \return The decomposed index triple of the bin for the given point
221 : 4 : details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const
222 : : {
223 : 4 : return this->impl().index3_(x, y, z);
224 : : }
225 : : /// \brief Compute the composed single index given a decomposed index
226 : : /// \param[in] idx A decomposed index triple for a bin
227 : : /// \return The composed bin index
228 : 46 : Index index(const details::Index3 & idx) const
229 : : {
230 : 46 : return this->impl().index_(idx);
231 : : }
232 : : /// \brief Compute the squared distance between the two points
233 : : /// \tparam PointT A point type with float members x, y and z, or point adapters defined
234 : : /// \param[in] x The x component of the first point
235 : : /// \param[in] y The y component of the first point
236 : : /// \param[in] z The z component of the first point
237 : : /// \param[in] pt The other point being compared
238 : : /// \return The squared distance between the points (2d or 3d)
239 : : template<typename PointT>
240 : 192 : float32_t distance_squared(
241 : : const float32_t x,
242 : : const float32_t y,
243 : : const float32_t z,
244 : : const PointT & pt) const
245 : : {
246 : 192 : return this->impl().distance_squared_(x, y, z, pt);
247 : : }
248 : :
249 : : protected:
250 : : /// \brief Computes the index in the x basis direction
251 : : /// \param[in] x The x value of a point
252 : : /// \return The x offset of the index
253 : 1480 : Index x_index(const float32_t x) const
254 : : {
255 : : return static_cast<Index>(
256 : 1480 : std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv));
257 : : }
258 : : /// \brief Computes the index in the y basis direction
259 : : /// \param[in] y The y value of a point
260 : : /// \return The x offset of the index
261 : 1480 : Index y_index(const float32_t y) const
262 : : {
263 : : return static_cast<Index>(
264 : 1480 : std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv));
265 : : }
266 : : /// \brief Computes the index in the z basis direction
267 : : /// \param[in] z The z value of a point
268 : : /// \return The x offset of the index
269 : 161 : Index z_index(const float32_t z) const
270 : : {
271 : : return static_cast<Index>(
272 : 161 : std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv));
273 : : }
274 : : /// \brief Compose the provided index offsets
275 : 6698 : Index bin_impl(const Index xdx, const Index ydx) const
276 : : {
277 : 6698 : return xdx + (ydx * m_y_stride);
278 : : }
279 : : /// \brief Compose the provided index offsets
280 : 27 : Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const
281 : : {
282 : 27 : return bin_impl(xdx, ydx) + (zdx * m_z_stride);
283 : : }
284 : :
285 : : /// \brief The index offset of a point given it's x and y values
286 : : /// \param[in] x The x value of a point
287 : : /// \param[in] y the y value of a point
288 : : /// \return The index of the point in the 2D case, or the offset within a z-slice of the hash
289 : 829 : Index bin_impl(const float32_t x, const float32_t y) const
290 : : {
291 : 829 : return bin_impl(x_index(x), y_index(y));
292 : : }
293 : : /// \brief The index of a point given it's x, y and z values
294 : : /// \param[in] x The x value of a point
295 : : /// \param[in] y the y value of a point
296 : : /// \param[in] z the z value of a point
297 : : /// \return The index of the bin for the specified point
298 : 160 : Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const
299 : : {
300 : 160 : return bin_impl(x, y) + (z_index(z) * m_z_stride);
301 : : }
302 : : /// \brief The distance between two indices as a float, where adjacent indices have zero
303 : : /// distance (e.g. dist(0, 1) = 0)
304 : 11765 : float32_t idx_distance(const Index ref_idx, const Index query_idx) const
305 : : {
306 : : /// Not using fabs because Index is (possibly) unsigned
307 [ + + ]: 11765 : const Index idist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx);
308 : 11765 : float32_t dist = static_cast<float32_t>(idist) - 1.0F;
309 : 11765 : return std::max(dist, 0.0F);
310 : : }
311 : :
312 : : /// \brief Get side length squared
313 : 5869 : float side_length2() const
314 : : {
315 : 5869 : return m_side_length2;
316 : : }
317 : :
318 : : private:
319 : : /// \brief Sanity check a range in a basis direction
320 : : /// \return The number of voxels in the given basis direction
321 : 69 : Index check_basis_direction(const float32_t min, const float32_t max) const
322 : : {
323 [ + + ]: 69 : if (min >= max) {
324 [ + - ]: 3 : 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 : 66 : const float64_t dmax = static_cast<float64_t>(max);
329 : 66 : const float64_t dmin = static_cast<float64_t>(min);
330 : 66 : const float64_t width = (dmax - dmin) * static_cast<float64_t>(m_side_length_inv);
331 : 66 : constexpr float64_t fltmax = static_cast<float64_t>(std::numeric_limits<float32_t>::max());
332 [ + + ]: 66 : if (fltmax <= width) {
333 [ + - ]: 3 : throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit");
334 : : }
335 : 63 : 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 : :
354 : : /// \brief Configuration class for a 2d spatial hash
355 : : class GEOMETRY_PUBLIC Config2d : public Config<Config2d>
356 : : {
357 : : public:
358 : : /// \brief Config constructor for 2D spatial hash
359 : : /// \param[in] min_x The minimum x value for the spatial hash
360 : : /// \param[in] max_x The maximum x value for the spatial hash
361 : : /// \param[in] min_y The minimum y value for the spatial hash
362 : : /// \param[in] max_y The maximum y value for the spatial hash
363 : : /// \param[in] radius The lookup distance
364 : : /// \param[in] capacity The maximum number of points the spatial hash can store
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);
372 : : /// \brief The index of a point given it's x, y and z values, 2d implementation
373 : : /// \param[in] x The x value of a point
374 : : /// \param[in] y the y value of a point
375 : : /// \param[in] z the z value of a point
376 : : /// \return The index of the bin for the specified point
377 : : Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
378 : : /// \brief Determine if a bin could possibly hold a point within a distance to any point in a
379 : : /// reference bin for the 2D case
380 : : /// \param[in] ref The decomposed index triple of the reference bin
381 : : /// \param[in] query The decomposed index triple of the bin being queried
382 : : /// \param[in] ref_distance2 The squared threshold distance
383 : : /// \return True if the reference bin and query bin could possibly hold a point within the
384 : : /// reference distance
385 : : bool valid(
386 : : const details::Index3 & ref,
387 : : const details::Index3 & query,
388 : : const float ref_distance2) const;
389 : : /// \brief Compute the decomposed index given a point, 2d implementation
390 : : /// \param[in] x The x component of the point
391 : : /// \param[in] y The y component of the point
392 : : /// \param[in] z The z component of the point
393 : : /// \return The decomposed index triple of the bin for the given point
394 : : details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
395 : : /// \brief Compute the composed single index given a decomposed index, 2d implementation
396 : : /// \param[in] idx A decomposed index triple for a bin
397 : : /// \return The composed bin index
398 : : Index index_(const details::Index3 & idx) const;
399 : : /// \brief Compute the squared distance between the two points, 2d implementation
400 : : /// \tparam PointT A point type with float members x, y and z, or point adapters defined
401 : : /// \param[in] x The x component of the first point
402 : : /// \param[in] y The y component of the first point
403 : : /// \param[in] z The z component of the first point
404 : : /// \param[in] pt The other point being compared
405 : : /// \return The squared distance between the points (2d)
406 : : template<typename PointT>
407 : 32 : float32_t distance_squared_(
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 : 32 : const float32_t dx = x - point_adapter::x_(pt);
415 : 32 : const float32_t dy = y - point_adapter::y_(pt);
416 : 32 : return (dx * dx) + (dy * dy);
417 : : }
418 : : }; // class Config2d
419 : :
420 : : /// \brief Configuration class for a 3d spatial hash
421 : : class GEOMETRY_PUBLIC Config3d : public Config<Config3d>
422 : : {
423 : : public:
424 : : /// \brief Config constructor for a 3d spatial hash
425 : : /// \param[in] min_x The minimum x value for the spatial hash
426 : : /// \param[in] max_x The maximum x value for the spatial hash
427 : : /// \param[in] min_y The minimum y value for the spatial hash
428 : : /// \param[in] max_y The maximum y value for the spatial hash
429 : : /// \param[in] min_z The minimum z value for the spatial hash
430 : : /// \param[in] max_z The maximum z value for the spatial hash
431 : : /// \param[in] radius The lookup distance
432 : : /// \param[in] capacity The maximum number of points the spatial hash can store
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);
442 : : /// \brief The index of a point given it's x, y and z values, 3d implementation
443 : : /// \param[in] x The x value of a point
444 : : /// \param[in] y the y value of a point
445 : : /// \param[in] z the z value of a point
446 : : /// \return The index of the bin for the specified point
447 : : Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
448 : : /// \brief Determine if a bin could possibly hold a point within a distance to any point in a
449 : : /// reference bin for the 3D case
450 : : /// \param[in] ref The decomposed index triple of the reference bin
451 : : /// \param[in] query The decomposed index triple of the bin being queried
452 : : /// \param[in] ref_distance2 The squared threshold distance
453 : : /// \return True if the reference bin and query bin could possibly hold a point within the
454 : : /// reference distance
455 : : bool valid(
456 : : const details::Index3 & ref,
457 : : const details::Index3 & query,
458 : : const float ref_distance2) const;
459 : : /// \brief Compute the decomposed index given a point, 3d implementation
460 : : /// \param[in] x The x component of the point
461 : : /// \param[in] y The y component of the point
462 : : /// \param[in] z The z component of the point
463 : : /// \return The decomposed index triple of the bin for the given point
464 : : details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
465 : : /// \brief Compute the composed single index given a decomposed index, 3d implementation
466 : : /// \param[in] idx A decomposed index triple for a bin
467 : : /// \return The composed bin index
468 : : Index index_(const details::Index3 & idx) const;
469 : : /// \brief Compute the squared distance between the two points, 3d implementation
470 : : /// \tparam PointT A point type with float members x, y and z, or point adapters defined
471 : : /// \param[in] x The x component of the first point
472 : : /// \param[in] y The y component of the first point
473 : : /// \param[in] z The z component of the first point
474 : : /// \param[in] pt The other point being compared
475 : : /// \return The squared distance between the points (3d)
476 : : template<typename PointT>
477 : 160 : float32_t distance_squared_(
478 : : const float32_t x,
479 : : const float32_t y,
480 : : const float32_t z,
481 : : const PointT & pt) const
482 : : {
483 : 160 : const float32_t dx = x - point_adapter::x_(pt);
484 : 160 : const float32_t dy = y - point_adapter::y_(pt);
485 : 160 : const float32_t dz = z - point_adapter::z_(pt);
486 : 160 : 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_
|
