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: 73 75 97.3 %
Date: 2021-01-26 05:02:50 Functions: 40 40 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 21 42 50.0 %

           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                 :        182 :     }
      74                 :            :     /// \brief Get stored point
      75                 :            :     /// \return A const reference to the stored point
      76                 :        182 :     const PointT & get_point() const
      77                 :            :     {
      78                 :        182 :       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                 :        182 :     operator const PointT &() const
      89                 :            :     {
      90                 :        182 :       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                 :        182 :     float32_t get_distance() const
     101                 :            :     {
     102                 :        182 :       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                 :          3 :   }
     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 Reset the state of the data structure
     153                 :          2 :   void clear()
     154                 :            :   {
     155                 :          2 :     m_hash.clear();
     156                 :          2 :   }
     157                 :            :   /// \brief Get current number of element stored in this data structure
     158                 :            :   /// \return Number of stored elements
     159                 :         30 :   Index size() const
     160                 :            :   {
     161                 :         30 :     return m_hash.size();
     162                 :            :   }
     163                 :            :   /// \brief Get the maximum capacity of the data structure
     164                 :            :   /// \return The capacity of the data structure
     165                 :         27 :   Index capacity() const
     166                 :            :   {
     167                 :         27 :     return m_config.get_capacity();
     168                 :            :   }
     169                 :            :   /// \brief Whether the hash is empty
     170                 :            :   /// \return True if data structure is empty
     171                 :          4 :   bool8_t empty() const
     172                 :            :   {
     173                 :          4 :     return m_hash.empty();
     174                 :            :   }
     175                 :            :   /// \brief Get iterator to beginning of data structure
     176                 :            :   /// \return Iterator
     177                 :          4 :   IT begin() const
     178                 :            :   {
     179                 :          4 :     return m_hash.begin();
     180                 :            :   }
     181                 :            :   /// \brief Get iterator to end of data structure
     182                 :            :   /// \return Iterator
     183                 :        174 :   IT end() const
     184                 :            :   {
     185                 :        174 :     return m_hash.end();
     186                 :            :   }
     187                 :            :   /// \brief Get iterator to beginning of data structure
     188                 :            :   /// \return Iterator
     189                 :          2 :   IT cbegin() const
     190                 :            :   {
     191                 :          2 :     return begin();
     192                 :            :   }
     193                 :            :   /// \brief Get iterator to end of data structure
     194                 :            :   /// \return Iterator
     195                 :        172 :   IT cend() const
     196                 :            :   {
     197                 :        172 :     return end();
     198                 :            :   }
     199                 :            : 
     200                 :            :   /// \brief Get the number of bins touched during the lifetime of this object, for debugging and
     201                 :            :   ///        size tuning
     202                 :            :   /// \return The total number of bins touched during near() queries
     203                 :          1 :   Index bins_hit() const
     204                 :            :   {
     205                 :          1 :     return m_bins_hit;
     206                 :            :   }
     207                 :            : 
     208                 :            :   /// \brief Get number of near neighbors found during the lifetime of this object, for debugging
     209                 :            :   ///        and size tuning
     210                 :            :   /// \return The total number of neighbors found during near() queries
     211                 :          1 :   Index neighbors_found() const
     212                 :            :   {
     213                 :          1 :     return m_neighbors_found;
     214                 :            :   }
     215                 :            : 
     216                 :            : protected:
     217                 :            :   /// \brief Finds all points within a fixed radius of a reference point
     218                 :            :   /// \param[in] x The x component of the reference point
     219                 :            :   /// \param[in] y The y component of the reference point
     220                 :            :   /// \param[in] z The z component of the reference point, respected only if the spatial hash is not
     221                 :            :   ///              2D.
     222                 :            :   /// \return A const reference to a vector containing iterators pointing to
     223                 :            :   ///         all points within the radius, and the actual distance to the reference point
     224                 :          3 :   const OutputVector & near_impl(
     225                 :            :     const float32_t x,
     226                 :            :     const float32_t y,
     227                 :            :     const float32_t z)
     228                 :            :   {
     229                 :            :     // reset output
     230         [ #  # ]:          3 :     m_neighbors.clear();
     231                 :            :     // Compute bin, bin range
     232         [ +  - ]:          3 :     const Index3 ref_idx = m_config.index3(x, y, z);
     233         [ +  - ]:          3 :     const details::BinRange idx_range = m_config.bin_range(ref_idx);
     234                 :          3 :     Index3 idx = idx_range.first;
     235                 :            :     // For bins in radius
     236         [ +  + ]:         37 :     do {  // guaranteed to have at least the bin ref_idx is in
     237                 :            :       // update book-keeping
     238                 :         37 :       ++m_bins_hit;
     239                 :            :       // Iterating in a square/cube pattern is easier than constructing sphere pattern
     240   [ +  -  +  - ]:         37 :       if (m_config.is_candidate_bin(ref_idx, idx)) {
     241                 :            :         // For point in bin
     242         [ +  - ]:         37 :         const Index jdx = m_config.index(idx);
     243         [ +  - ]:         37 :         const auto range = m_hash.equal_range(jdx);
     244         [ +  + ]:        219 :         for (auto it = range.first; it != range.second; ++it) {
     245                 :        182 :           const auto & pt = it->second;
     246         [ +  - ]:        182 :           const float32_t dist2 = m_config.distance_squared(x, y, z, pt);
     247         [ +  - ]:        182 :           if (dist2 <= m_config.radius2()) {
     248                 :            :             // Only compute true distance if necessary
     249         [ +  - ]:        182 :             m_neighbors.emplace_back(it, sqrtf(dist2));
     250                 :            :           }
     251                 :            :         }
     252                 :            :       }
     253                 :            :     } while (m_config.next_bin(idx_range, idx));
     254                 :            :     // update book-keeping
     255                 :          3 :     m_neighbors_found += m_neighbors.size();
     256                 :          3 :     return m_neighbors;
     257                 :            :   }
     258                 :            : 
     259                 :            : private:
     260                 :            :   /// \brief Internal insert method with no error checking
     261                 :            :   /// \param[in] pt The Point to insert
     262                 :        182 :   GEOMETRY_LOCAL IT insert_impl(const PointT & pt)
     263                 :            :   {
     264                 :            :     // Compute bin
     265         [ +  - ]:        182 :     const Index idx =
     266                 :            :       m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt));
     267                 :            :     // Insert into bin
     268   [ +  -  +  - ]:        182 :     return m_hash.insert(std::make_pair(idx, pt));
     269                 :            :   }
     270                 :            : 
     271                 :            :   const ConfigT m_config;
     272                 :            :   Hash m_hash;
     273                 :            :   OutputVector m_neighbors;
     274                 :            :   Index m_bins_hit;
     275                 :            :   Index m_neighbors_found;
     276                 :            : };  // class SpatialHashBase
     277                 :            : 
     278                 :            : /// \brief The class to be used for specializing on
     279                 :            : /// apex_app::common::geometry::spatial_hash::SpatialHashBase to provide different function
     280                 :            : /// signatures on 2D and 3D configurations
     281                 :            : /// \tparam PointT The point type stored in this data structure. Must have float members x, y and z
     282                 :            : template<typename PointT, typename ConfigT>
     283                 :            : class GEOMETRY_PUBLIC SpatialHash;
     284                 :            : 
     285                 :            : /// \brief Explicit specialization of SpatialHash for 2D configuration
     286                 :            : /// \tparam PointT The point type stored in this data structure.
     287                 :            : template<typename PointT>
     288                 :            : class GEOMETRY_PUBLIC SpatialHash<PointT, Config2d>: public SpatialHashBase<PointT, Config2d>
     289                 :            : {
     290                 :            : public:
     291                 :            :   using OutputVector = typename SpatialHashBase<PointT, Config2d>::OutputVector;
     292                 :            : 
     293                 :          2 :   explicit SpatialHash(const Config2d & cfg)
     294                 :          2 :   : SpatialHashBase<PointT, Config2d>(cfg) {}
     295                 :            : 
     296                 :            :   /// \brief Finds all points within a fixed radius of a reference point
     297                 :            :   /// \param[in] x The x component of the reference point
     298                 :            :   /// \param[in] y The y component of the reference point
     299                 :            :   /// \return A const reference to a vector containing iterators pointing to
     300                 :            :   ///         all points within the configured radius, and the actual distance to the reference
     301                 :            :   ///         point
     302                 :          2 :   const OutputVector & near(const float32_t x, const float32_t y)
     303                 :            :   {
     304                 :          2 :     return this->near_impl(x, y, 0.0F);
     305                 :            :   }
     306                 :            : 
     307                 :            :   /// \brief Finds all points within a fixed radius of a reference point
     308                 :            :   /// \param[in] pt The reference point. Only the x and y members are respected.
     309                 :            :   /// \return A const reference to a vector containing iterators pointing to
     310                 :            :   ///         all points within the configured radius, and the actual distance to the reference
     311                 :            :   ///         point
     312                 :          2 :   const OutputVector & near(const PointT & pt)
     313                 :            :   {
     314                 :          2 :     return near(point_adapter::x_(pt), point_adapter::y_(pt));
     315                 :            :   }
     316                 :            : };
     317                 :            : 
     318                 :            : /// \brief Explicit specialization of SpatialHash for 3D configuration
     319                 :            : /// \tparam PointT The point type stored in this data structure. Must have float members x, y and z
     320                 :            : template<typename PointT>
     321                 :            : class GEOMETRY_PUBLIC SpatialHash<PointT, Config3d>: public SpatialHashBase<PointT, Config3d>
     322                 :            : {
     323                 :            : public:
     324                 :            :   using OutputVector = typename SpatialHashBase<PointT, Config3d>::OutputVector;
     325                 :            : 
     326                 :          1 :   explicit SpatialHash(const Config3d & cfg)
     327                 :          1 :   : SpatialHashBase<PointT, Config3d>(cfg) {}
     328                 :            : 
     329                 :            :   /// \brief Finds all points within a fixed radius of a reference point
     330                 :            :   /// \param[in] x The x component of the reference point
     331                 :            :   /// \param[in] y The y component of the reference point
     332                 :            :   /// \param[in] z The z component of the reference point, respected only if the spatial hash is not
     333                 :            :   ///              2D.
     334                 :            :   /// \return A const reference to a vector containing iterators pointing to
     335                 :            :   ///         all points within the configured radius, and the actual distance to the reference
     336                 :            :   ///         point
     337                 :          1 :   const OutputVector & near(const float32_t x, const float32_t y, const float32_t z)
     338                 :            :   {
     339                 :          1 :     return this->near_impl(x, y, z);
     340                 :            :   }
     341                 :            : 
     342                 :            :   /// \brief Finds all points within a fixed radius of a reference point
     343                 :            :   /// \param[in] pt The reference point.
     344                 :            :   /// \return A const reference to a vector containing iterators pointing to
     345                 :            :   ///         all points within the configured radius, and the actual distance to the reference
     346                 :            :   ///         point
     347                 :          1 :   const OutputVector & near(const PointT & pt)
     348                 :            :   {
     349                 :          1 :     return near(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt));
     350                 :            :   }
     351                 :            : };
     352                 :            : 
     353                 :            : template<typename T>
     354                 :            : using SpatialHash2d = SpatialHash<T, Config2d>;
     355                 :            : template<typename T>
     356                 :            : using SpatialHash3d = SpatialHash<T, Config3d>;
     357                 :            : }  // namespace spatial_hash
     358                 :            : }  // namespace geometry
     359                 :            : }  // namespace common
     360                 :            : }  // namespace autoware
     361                 :            : 
     362                 :            : #endif  // GEOMETRY__SPATIAL_HASH_HPP_

Generated by: LCOV version 1.14