spatial_hash_config.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_CONFIG_HPP_
#define GEOMETRY__SPATIAL_HASH_CONFIG_HPP_

#include <common/types.hpp>
#include <geometry/visibility_control.hpp>
#include <geometry/common_2d.hpp>

#include <algorithm>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <utility>

#include "helper_functions/crtp.hpp"

using autoware::common::types::float64_t;
using autoware::common::types::float32_t;
using autoware::common::types::bool8_t;

namespace autoware
{
namespace common
{
namespace geometry
{
namespace spatial_hash
{
using Index = std::size_t;
namespace details
{
struct GEOMETRY_PUBLIC Index3
{
  Index x;
  Index y;
  Index z;
};  // struct Index3

using BinRange = std::pair<Index3, Index3>;
}  // namespace details

template<typename Derived>
class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<Derived>
{
public:
  Config(
    const float32_t min_x,
    const float32_t max_x,
    const float32_t min_y,
    const float32_t max_y,
    const float32_t min_z,
    const float32_t max_z,
    const float32_t radius,
    const Index capacity)
  : m_min_x{min_x},
    m_min_y{min_y},
    m_min_z{min_z},
    m_max_x{max_x},
    m_max_y{max_y},
    m_max_z{max_z},
    m_side_length{radius},
    m_side_length2{radius * radius},
    m_side_length_inv{1.0F / radius},
    m_capacity{capacity},
    m_max_x_idx{check_basis_direction(min_x, max_x)},
    m_max_y_idx{check_basis_direction(min_y, max_y)},
    m_max_z_idx{check_basis_direction(min_z, max_z)},
    m_y_stride{m_max_x_idx + 1U},
    m_z_stride{m_y_stride * (m_max_y_idx + 1U)}
  {
    if (radius <= 0.0F) {
      throw std::domain_error("Error constructing SpatialHash: must have positive side length");
    }

    if ((m_max_y_idx + m_y_stride) > std::numeric_limits<Index>::max() / 2U) {
      // TODO(c.ho) properly check for multiplication overflow
      throw std::domain_error("SpatialHash::Config: voxel index may overflow!");
    }
    // small fudging to prevent weird boundary effects
    // (e.g (x=xmax, y) rolls index over to (x=0, y+1)
    constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
    //lint -e{1938} read only access is fine NOLINT
    m_max_x -= FEPS;
    m_max_y -= FEPS;
    m_max_z -= FEPS;
    if ((m_z_stride + m_max_z_idx) > std::numeric_limits<Index>::max() / 2U) {
      // TODO(c.ho) properly check for multiplication overflow
      throw std::domain_error("SpatialHash::Config: voxel index may overflow!");
    }
    // I don't know if this is even possible given other checks
    if (std::numeric_limits<Index>::max() == m_max_z_idx) {
      throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value");
    }
  }

  details::BinRange bin_range(const details::Index3 & ref) const
  {
    // Compute distance in units of voxels
    constexpr Index iradius = 1;
    // Dumb ternary because potentially unsigned Index type
    const Index xmin = (ref.x > iradius) ? (ref.x - iradius) : 0U;
    const Index ymin = (ref.y > iradius) ? (ref.y - iradius) : 0U;
    const Index zmin = (ref.z > iradius) ? (ref.z - iradius) : 0U;
    // In 2D mode, m_max_z should be 0, same with ref.z
    const Index xmax = std::min(ref.x + iradius, m_max_x_idx);
    const Index ymax = std::min(ref.y + iradius, m_max_y_idx);
    const Index zmax = std::min(ref.z + iradius, m_max_z_idx);
    // return bottom-left portion of cube and top-right portion of cube
    return {{xmin, ymin, zmin}, {xmax, ymax, zmax}};
  }
  bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const
  {
    // TODO(c.ho) is there any way to make this neater without triple nested if?
    bool8_t ret = true;
    ++idx.x;
    if (idx.x > range.second.x) {
      idx.x = range.first.x;
      ++idx.y;
      if (idx.y > range.second.y) {
        idx.y = range.first.y;
        ++idx.z;
        if (idx.z > range.second.z) {
          ret = false;
        }
      }
    }
    return ret;
  }
  Index get_capacity() const
  {
    return m_capacity;
  }

  float32_t radius2() const
  {
    return m_side_length2;
  }

  // "Polymorphic" API
  Index bin(const float32_t x, const float32_t y, const float32_t z) const
  {
    return this->impl().bin_(x, y, z);
  }
  bool8_t is_candidate_bin(const details::Index3 & ref, const details::Index3 & query) const
  {
    return this->impl().valid(ref, query);
  }
  details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const
  {
    return this->impl().index3_(x, y, z);
  }
  Index index(const details::Index3 & idx) const
  {
    return this->impl().index_(idx);
  }
  template<typename PointT>
  float32_t distance_squared(
    const float32_t x,
    const float32_t y,
    const float32_t z,
    const PointT & pt) const
  {
    return this->impl().distance_squared_(x, y, z, pt);
  }

protected:
  Index x_index(const float32_t x) const
  {
    return static_cast<Index>(
      std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv));
  }
  Index y_index(const float32_t y) const
  {
    return static_cast<Index>(
      std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv));
  }
  Index z_index(const float32_t z) const
  {
    return static_cast<Index>(
      std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv));
  }
  Index bin_impl(const Index xdx, const Index ydx) const
  {
    return xdx + (ydx * m_y_stride);
  }
  Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const
  {
    return bin_impl(xdx, ydx) + (zdx * m_z_stride);
  }

  Index bin_impl(const float32_t x, const float32_t y) const
  {
    return bin_impl(x_index(x), y_index(y));
  }
  Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const
  {
    return bin_impl(x, y) + (z_index(z) * m_z_stride);
  }
  float32_t idx_distance(const Index ref_idx, const Index query_idx) const
  {
    const Index idist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx);
    float32_t dist = static_cast<float32_t>(idist) - 1.0F;
    return std::max(dist, 0.0F);
  }

private:
  Index check_basis_direction(const float32_t min, const float32_t max) const
  {
    if (min >= max) {
      throw std::domain_error("SpatialHash::Config: must have min < max");
    }
    // This family of checks is to ensure that you don't get weird casting effects due to huge
    // floating point values
    const float64_t dmax = static_cast<float64_t>(max);
    const float64_t dmin = static_cast<float64_t>(min);
    const float64_t width = (dmax - dmin) * static_cast<float64_t>(m_side_length_inv);
    constexpr float64_t fltmax = static_cast<float64_t>(std::numeric_limits<float32_t>::max());
    if (fltmax <= width) {
      throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit");
    }
    return static_cast<Index>(width);
  }
  float32_t m_min_x;
  float32_t m_min_y;
  float32_t m_min_z;
  float32_t m_max_x;
  float32_t m_max_y;
  float32_t m_max_z;
  float32_t m_side_length;
  float32_t m_side_length2;
  float32_t m_side_length_inv;
  Index m_capacity;
  Index m_max_x_idx;
  Index m_max_y_idx;
  Index m_max_z_idx;
  Index m_y_stride;
  Index m_z_stride;
};  // class Config

class GEOMETRY_PUBLIC Config2d : public Config<Config2d>
{
public:
  Config2d(
    const float32_t min_x,
    const float32_t max_x,
    const float32_t min_y,
    const float32_t max_y,
    const float32_t radius,
    const Index capacity);
  Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
  bool8_t valid(const details::Index3 & ref, const details::Index3 & query) const;
  details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
  Index index_(const details::Index3 & idx) const;
  template<typename PointT>
  float32_t distance_squared_(
    const float32_t x,
    const float32_t y,
    const float32_t z,
    const PointT & pt) const
  {
    (void)z;
    const float32_t dx = x - point_adapter::x_(pt);
    const float32_t dy = y - point_adapter::y_(pt);
    return (dx * dx) + (dy * dy);
  }
};  // class Config2d

class GEOMETRY_PUBLIC Config3d : public Config<Config3d>
{
public:
  Config3d(
    const float32_t min_x,
    const float32_t max_x,
    const float32_t min_y,
    const float32_t max_y,
    const float32_t min_z,
    const float32_t max_z,
    const float32_t radius,
    const Index capacity);
  Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
  bool8_t valid(const details::Index3 & ref, const details::Index3 & query) const;
  details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
  Index index_(const details::Index3 & idx) const;
  template<typename PointT>
  float32_t distance_squared_(
    const float32_t x,
    const float32_t y,
    const float32_t z,
    const PointT & pt) const
  {
    const float32_t dx = x - point_adapter::x_(pt);
    const float32_t dy = y - point_adapter::y_(pt);
    const float32_t dz = z - point_adapter::z_(pt);
    return (dx * dx) + (dy * dy) + (dz * dz);
  }
};  // class Config3d
}  // namespace spatial_hash
}  // namespace geometry
}  // namespace common
}  // namespace autoware

#endif  // GEOMETRY__SPATIAL_HASH_CONFIG_HPP_