LCOV - code coverage report
Current view: top level - src/common/autoware_auto_geometry/include/geometry - spatial_hash.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 37 47 78.7 %
Date: 2023-03-03 05:44:19 Functions: 6 12 50.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 19 36 52.8 %

           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_

Generated by: LCOV version 1.14