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_HPP_
21 : : #define GEOMETRY__SPATIAL_HASH_HPP_
22 : :
23 : : #include <common/types.hpp>
24 : : #include <geometry/spatial_hash_config.hpp>
25 : : #include <geometry/visibility_control.hpp>
26 : : #include <vector>
27 : : #include <unordered_map>
28 : : #include <utility>
29 : :
30 : : using autoware::common::types::float32_t;
31 : : using autoware::common::types::bool8_t;
32 : :
33 : : namespace autoware
34 : : {
35 : : namespace common
36 : : {
37 : : namespace geometry
38 : : {
39 : : /// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup
40 : : namespace spatial_hash
41 : : {
42 : :
43 : : /// \brief An implementation of the spatial hash or integer lattice data structure for efficient
44 : : /// (O(1)) near neighbor queries.
45 : : /// \tparam PointT The point type stored in this data structure. Must have float members x, y, and z
46 : : ///
47 : : /// This implementation can support both 2D and 3D queries
48 : : /// (though only one type per data structure), and can support queries of varying radius. This data
49 : : /// structure cannot do near neighbor lookups for euclidean distance in arbitrary dimensions.
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;
62 : : /// \brief Wrapper around an iterator and a distance (from some query point)
63 : : class Output
64 : : {
65 : : public:
66 : : /// \brief Constructor
67 : : /// \param[in] iterator An iterator pointing to some point
68 : : /// \param[in] distance The euclidean distance (2d or 3d) to a reference point
69 : 182 : Output(const IT iterator, const float32_t distance)
70 : : : m_iterator(iterator),
71 : 182 : m_distance(distance)
72 : : {
73 : : }
74 : : /// \brief Get stored point
75 : : /// \return A const reference to the stored point
76 : : const PointT & get_point() const
77 : : {
78 [ + - + - ]: 170 : return m_iterator->second;
79 : : }
80 : : /// \brief Get underlying iterator
81 : : /// \return A copy of the underlying iterator
82 : : IT get_iterator() const
83 : : {
84 : : return m_iterator;
85 : : }
86 : : /// \brief Convert to underlying point
87 : : /// \return A reference to the underlying point
88 : : operator const PointT &() const
89 : : {
90 : : return get_point();
91 : : }
92 : : /// \brief Convert to underlying iterator
93 : : /// \return A copy of the iterator
94 : : operator IT() const
95 : : {
96 : : return get_iterator();
97 : : }
98 : : /// \brief Get distance to reference point
99 : : /// \return The distance
100 : : float32_t get_distance() const
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 : :
111 : : /// \brief Constructor
112 : : /// \param[in] cfg The configuration object for this class
113 : 3 : 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 [ + - + - ]: 3 : m_neighbors_found{}
119 : : {
120 : : }
121 : :
122 : : /// \brief Inserts point
123 : : /// \param[in] pt The Point to insert
124 : : /// \return Iterator pointing to the inserted point
125 : : /// \throw std::length_error If the data structure is at capacity
126 [ - + ]: 22 : IT insert(const PointT & pt)
127 : : {
128 : 22 : if (size() >= capacity()) {
129 [ # # ]: 0 : throw std::length_error{"SpatialHash: Cannot insert past capacity"};
130 : : }
131 : 22 : return insert_impl(pt);
132 : : }
133 : :
134 : : /// \brief Inserts a range of points
135 : : /// \param[in] begin The start of the range of points to insert
136 : : /// \param[in] end The end of the range of points to insert
137 : : /// \tparam IteratorT The iterator type
138 : : /// \throw std::length_error If the range of points to insert exceeds the data structure's
139 : : /// capacity
140 : : template<typename IteratorT>
141 [ + - ]: 5 : void insert(IteratorT begin, IteratorT end)
142 : : {
143 : : // This check is here for strong exception safety
144 : 5 : if ((size() + std::distance(begin, end)) > capacity()) {
145 [ # # ]: 0 : throw std::length_error{"SpatialHash: Cannot multi-insert past capacity"};
146 : : }
147 [ + + ]: 165 : for (IteratorT it = begin; it != end; ++it) {
148 : 160 : (void)insert_impl(*it);
149 : : }
150 : 5 : }
151 : :
152 : : /// \brief Removes the specified element from the data structure
153 : : /// \param[in] point An iterator pointing to a point to be removed
154 : : /// \return An iterator pointing to the element after the erased element
155 : : /// \throw std::domain_error If pt is invalid or does not belong to this data structure
156 : : ///
157 : : /// \note There is no reliable way to check if an iterator is invalid. The checks here are
158 : : /// based on a heuristic and is not guaranteed to find all invalid iterators. This method
159 : : /// should be used with care and only on valid iterators
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 : :
168 : : /// \brief Reset the state of the data structure
169 : : void clear()
170 : : {
171 : : m_hash.clear();
172 : : }
173 : : /// \brief Get current number of element stored in this data structure
174 : : /// \return Number of stored elements
175 : : Index size() const
176 : : {
177 : : return m_hash.size();
178 : : }
179 : : /// \brief Get the maximum capacity of the data structure
180 : : /// \return The capacity of the data structure
181 : : Index capacity() const
182 : : {
183 [ - + + - ]: 27 : return m_config.get_capacity();
184 : : }
185 : : /// \brief Whether the hash is empty
186 : : /// \return True if data structure is empty
187 : : bool8_t empty() const
188 : : {
189 : : return m_hash.empty();
190 : : }
191 : : /// \brief Get iterator to beginning of data structure
192 : : /// \return Iterator
193 : : IT begin() const
194 : : {
195 : : return m_hash.begin();
196 : : }
197 : : /// \brief Get iterator to end of data structure
198 : : /// \return Iterator
199 : : IT end() const
200 : : {
201 : : return m_hash.end();
202 : : }
203 : : /// \brief Get iterator to beginning of data structure
204 : : /// \return Iterator
205 : : IT cbegin() const
206 : : {
207 : : return begin();
208 : : }
209 : : /// \brief Get iterator to end of data structure
210 : : /// \return Iterator
211 : : IT cend() const
212 : : {
213 : : return end();
214 : : }
215 : :
216 : : /// \brief Get the number of bins touched during the lifetime of this object, for debugging and
217 : : /// size tuning
218 : : /// \return The total number of bins touched during near() queries
219 : : Index bins_hit() const
220 : : {
221 : : return m_bins_hit;
222 : : }
223 : :
224 : : /// \brief Get number of near neighbors found during the lifetime of this object, for debugging
225 : : /// and size tuning
226 : : /// \return The total number of neighbors found during near() queries
227 : : Index neighbors_found() const
228 : : {
229 : : return m_neighbors_found;
230 : : }
231 : :
232 : : protected:
233 : : /// \brief Finds all points within a fixed radius of a reference point
234 : : /// \param[in] x The x component of the reference point
235 : : /// \param[in] y The y component of the reference point
236 : : /// \param[in] z The z component of the reference point, respected only if the spatial hash is not
237 : : /// 2D.
238 : : /// \param[in] radius The radius within which to find all near points
239 : : /// \return A const reference to a vector containing iterators pointing to
240 : : /// all points within the radius, and the actual distance to the reference point
241 : 4 : const OutputVector & near_impl(
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 [ - + ]: 4 : m_neighbors.clear();
249 : : // Compute bin, bin range
250 : 4 : const Index3 ref_idx = m_config.index3(x, y, z);
251 : 4 : const float32_t radius2 = radius * radius;
252 : 4 : const details::BinRange idx_range = m_config.bin_range(ref_idx, radius);
253 : 4 : 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 : 46 : ++m_bins_hit;
258 : : // Iterating in a square/cube pattern is easier than constructing sphere pattern
259 [ + - ]: 46 : if (m_config.is_candidate_bin(ref_idx, idx, radius2)) {
260 : : // For point in bin
261 : 46 : const Index jdx = m_config.index(idx);
262 : : const auto range = m_hash.equal_range(jdx);
263 [ + + ]: 238 : 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 [ + + ]: 192 : if (dist2 <= radius2) {
267 : : // Only compute true distance if necessary
268 : 182 : m_neighbors.emplace_back(it, sqrtf(dist2));
269 : : }
270 : : }
271 : : }
272 : : } while (m_config.next_bin(idx_range, idx));
273 : : // update book-keeping
274 : 4 : m_neighbors_found += m_neighbors.size();
275 : 4 : return m_neighbors;
276 : : }
277 : :
278 : : private:
279 : : /// \brief Internal insert method with no error checking
280 : : /// \param[in] pt The Point to insert
281 : 182 : GEOMETRY_LOCAL IT insert_impl(const PointT & pt)
282 : : {
283 : : // Compute bin
284 : 182 : const Index idx =
285 : : m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt));
286 : : // Insert into bin
287 : 182 : 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 : :
297 : : /// \brief The class to be used for specializing on
298 : : /// apex_app::common::geometry::spatial_hash::SpatialHashBase to provide different function
299 : : /// signatures on 2D and 3D configurations
300 : : /// \tparam PointT The point type stored in this data structure. Must have float members x, y and z
301 : : template<typename PointT, typename ConfigT>
302 : : class GEOMETRY_PUBLIC SpatialHash;
303 : :
304 : : /// \brief Explicit specialization of SpatialHash for 2D configuration
305 : : /// \tparam PointT The point type stored in this data structure.
306 : : template<typename PointT>
307 : 2 : class GEOMETRY_PUBLIC SpatialHash<PointT, Config2d>: public SpatialHashBase<PointT, Config2d>
308 : : {
309 : : public:
310 : : using OutputVector = typename SpatialHashBase<PointT, Config2d>::OutputVector;
311 : :
312 : 0 : explicit SpatialHash(const Config2d & cfg)
313 : 0 : : SpatialHashBase<PointT, Config2d>(cfg) {}
314 : :
315 : : /// \brief Finds all points within a fixed radius of a reference point
316 : : /// \param[in] x The x component of the reference point
317 : : /// \param[in] y The y component of the reference point
318 : : /// \param[in] radius The radius within which to find all near points
319 : : /// \return A const reference to a vector containing iterators pointing to
320 : : /// all points within the radius, and the actual distance to the reference point
321 : 0 : const OutputVector & near(
322 : : const float32_t x,
323 : : const float32_t y,
324 : : const float32_t radius)
325 : : {
326 : 3 : return this->near_impl(x, y, 0.0F, radius);
327 : : }
328 : :
329 : : /// \brief Finds all points within a fixed radius of a reference point
330 : : /// \param[in] pt The reference point. Only the x and y members are respected.
331 : : /// \param[in] radius The radius within which to find all near points
332 : : /// \return A const reference to a vector containing iterators pointing to
333 : : /// all points within the radius, and the actual distance to the reference point
334 : 0 : const OutputVector & near(const PointT & pt, const float32_t radius)
335 : : {
336 [ + - + - ]: 3 : return near(point_adapter::x_(pt), point_adapter::y_(pt), radius);
337 : : }
338 : : };
339 : :
340 : : /// \brief Explicit specialization of SpatialHash for 3D configuration
341 : : /// \tparam PointT The point type stored in this data structure. Must have float members x, y and z
342 : : template<typename PointT>
343 : 1 : class GEOMETRY_PUBLIC SpatialHash<PointT, Config3d>: public SpatialHashBase<PointT, Config3d>
344 : : {
345 : : public:
346 : : using OutputVector = typename SpatialHashBase<PointT, Config3d>::OutputVector;
347 : :
348 : 0 : explicit SpatialHash(const Config3d & cfg)
349 : 0 : : SpatialHashBase<PointT, Config3d>(cfg) {}
350 : :
351 : : /// \brief Finds all points within a fixed radius of a reference point
352 : : /// \param[in] x The x component of the reference point
353 : : /// \param[in] y The y component of the reference point
354 : : /// \param[in] z The z component of the reference point, respected only if the spatial hash is not
355 : : /// 2D.
356 : : /// \param[in] radius The radius within which to find all near points
357 : : /// \return A const reference to a vector containing iterators pointing to
358 : : /// all points within the radius, and the actual distance to the reference point
359 : 0 : const OutputVector & near(
360 : : const float32_t x,
361 : : const float32_t y,
362 : : const float32_t z,
363 : : const float32_t radius)
364 : : {
365 : 1 : return this->near_impl(x, y, z, radius);
366 : : }
367 : :
368 : : /// \brief Finds all points within a fixed radius of a reference point
369 : : /// \param[in] pt The reference point.
370 : : /// \param[in] radius The radius within which to find all near points
371 : : /// \return A const reference to a vector containing iterators pointing to
372 : : /// all points within the radius, and the actual distance to the reference point
373 : 0 : const OutputVector & near(const PointT & pt, const float32_t radius)
374 : : {
375 : : return near(
376 : : point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt),
377 [ + - ]: 1 : 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_
|