spatial_hash.hpp

Go to the documentation of this file.

// Copyright 2019 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
 
#ifndef GEOMETRY__SPATIAL_HASH_HPP_
#define GEOMETRY__SPATIAL_HASH_HPP_
 
#include <common/types.hpp>
#include <geometry/spatial_hash_config.hpp>
#include <geometry/visibility_control.hpp>
#include <vector>
#include <unordered_map>
#include <utility>
 
using autoware::common::types::float32_t;
using autoware::common::types::bool8_t;
 
namespace autoware
{
namespace common
{
namespace geometry
{
namespace spatial_hash
{
 
template<typename PointT, typename ConfigT>
class GEOMETRY_PUBLIC SpatialHashBase
{
  using Index3 = details::Index3;
  //lint -e{9131} NOLINT There's no other way to make this work in a static assert
  static_assert(
    std::is_same<ConfigT, Config2d>::value || std::is_same<ConfigT, Config3d>::value,
    "SpatialHash only works with Config2d or Config3d");
 
public:
  using Hash = std::unordered_multimap<Index, PointT>;
  using IT = typename Hash::const_iterator;
  class Output
  {
public:
    Output(const IT iterator, const float32_t distance)
    : m_iterator(iterator),
      m_distance(distance)
    {
    }
    const PointT & get_point() const
    {
      return m_iterator->second;
    }
    IT get_iterator() const
    {
      return m_iterator;
    }
    operator const PointT &() const
    {
      return get_point();
    }
    operator IT() const
    {
      return get_iterator();
    }
    float32_t get_distance() const
    {
      return m_distance;
    }
 
private:
    IT m_iterator;
    float32_t m_distance;
  };  // class Output
  using OutputVector = typename std::vector<Output>;
 
  explicit SpatialHashBase(const ConfigT & cfg)
  : m_config{cfg},
    m_hash(),
    m_neighbors{},  // TODO(c.ho) reserve, but there's no default constructor for output
    m_bins_hit{},  // zero initialization (and below)
    m_neighbors_found{}
  {
  }
 
  IT insert(const PointT & pt)
  {
    if (size() >= capacity()) {
      throw std::length_error{"SpatialHash: Cannot insert past capacity"};
    }
    return insert_impl(pt);
  }
 
  template<typename IteratorT>
  void insert(IteratorT begin, IteratorT end)
  {
    // This check is here for strong exception safety
    if ((size() + std::distance(begin, end)) > capacity()) {
      throw std::length_error{"SpatialHash: Cannot multi-insert past capacity"};
    }
    for (IteratorT it = begin; it != end; ++it) {
      (void)insert_impl(*it);
    }
  }
 
  void clear()
  {
    m_hash.clear();
  }
  Index size() const
  {
    return m_hash.size();
  }
  Index capacity() const
  {
    return m_config.get_capacity();
  }
  bool8_t empty() const
  {
    return m_hash.empty();
  }
  IT begin() const
  {
    return m_hash.begin();
  }
  IT end() const
  {
    return m_hash.end();
  }
  IT cbegin() const
  {
    return begin();
  }
  IT cend() const
  {
    return end();
  }
 
  Index bins_hit() const
  {
    return m_bins_hit;
  }
 
  Index neighbors_found() const
  {
    return m_neighbors_found;
  }
 
protected:
  const OutputVector & near_impl(
    const float32_t x,
    const float32_t y,
    const float32_t z)
  {
    // reset output
    m_neighbors.clear();
    // Compute bin, bin range
    const Index3 ref_idx = m_config.index3(x, y, z);
    const details::BinRange idx_range = m_config.bin_range(ref_idx);
    Index3 idx = idx_range.first;
    // For bins in radius
    do {  // guaranteed to have at least the bin ref_idx is in
      // update book-keeping
      ++m_bins_hit;
      // Iterating in a square/cube pattern is easier than constructing sphere pattern
      if (m_config.is_candidate_bin(ref_idx, idx)) {
        // For point in bin
        const Index jdx = m_config.index(idx);
        const auto range = m_hash.equal_range(jdx);
        for (auto it = range.first; it != range.second; ++it) {
          const auto & pt = it->second;
          const float32_t dist2 = m_config.distance_squared(x, y, z, pt);
          if (dist2 <= m_config.radius2()) {
            // Only compute true distance if necessary
            m_neighbors.emplace_back(it, sqrtf(dist2));
          }
        }
      }
    } while (m_config.next_bin(idx_range, idx));
    // update book-keeping
    m_neighbors_found += m_neighbors.size();
    return m_neighbors;
  }
 
private:
  GEOMETRY_LOCAL IT insert_impl(const PointT & pt)
  {
    // Compute bin
    const Index idx =
      m_config.bin(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt));
    // Insert into bin
    return m_hash.insert(std::make_pair(idx, pt));
  }
 
  const ConfigT m_config;
  Hash m_hash;
  OutputVector m_neighbors;
  Index m_bins_hit;
  Index m_neighbors_found;
};  // class SpatialHashBase
 
template<typename PointT, typename ConfigT>
class GEOMETRY_PUBLIC SpatialHash;
 
template<typename PointT>
class GEOMETRY_PUBLIC SpatialHash<PointT, Config2d>: public SpatialHashBase<PointT, Config2d>
{
public:
  using OutputVector = typename SpatialHashBase<PointT, Config2d>::OutputVector;
 
  explicit SpatialHash(const Config2d & cfg)
  : SpatialHashBase<PointT, Config2d>(cfg) {}
 
  const OutputVector & near(const float32_t x, const float32_t y)
  {
    return this->near_impl(x, y, 0.0F);
  }
 
  const OutputVector & near(const PointT & pt)
  {
    return near(point_adapter::x_(pt), point_adapter::y_(pt));
  }
};
 
template<typename PointT>
class GEOMETRY_PUBLIC SpatialHash<PointT, Config3d>: public SpatialHashBase<PointT, Config3d>
{
public:
  using OutputVector = typename SpatialHashBase<PointT, Config3d>::OutputVector;
 
  explicit SpatialHash(const Config3d & cfg)
  : SpatialHashBase<PointT, Config3d>(cfg) {}
 
  const OutputVector & near(const float32_t x, const float32_t y, const float32_t z)
  {
    return this->near_impl(x, y, z);
  }
 
  const OutputVector & near(const PointT & pt)
  {
    return near(point_adapter::x_(pt), point_adapter::y_(pt), point_adapter::z_(pt));
  }
};
 
template<typename T>
using SpatialHash2d = SpatialHash<T, Config2d>;
template<typename T>
using SpatialHash3d = SpatialHash<T, Config3d>;
}  // namespace spatial_hash
}  // namespace geometry
}  // namespace common
}  // namespace autoware
 
#endif  // GEOMETRY__SPATIAL_HASH_HPP_