LCOV - code coverage report
Current view: top level - src/common/autoware_auto_geometry/include/geometry - spatial_hash_config.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 82 85 96.5 %
Date: 2021-01-26 05:02:50 Functions: 32 32 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 49 78 62.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_CONFIG_HPP_
      21                 :            : #define GEOMETRY__SPATIAL_HASH_CONFIG_HPP_
      22                 :            : 
      23                 :            : #include <common/types.hpp>
      24                 :            : #include <geometry/visibility_control.hpp>
      25                 :            : #include <geometry/common_2d.hpp>
      26                 :            : 
      27                 :            : #include <algorithm>
      28                 :            : #include <cmath>
      29                 :            : #include <limits>
      30                 :            : #include <stdexcept>
      31                 :            : #include <utility>
      32                 :            : 
      33                 :            : #include "helper_functions/crtp.hpp"
      34                 :            : 
      35                 :            : using autoware::common::types::float64_t;
      36                 :            : using autoware::common::types::float32_t;
      37                 :            : using autoware::common::types::bool8_t;
      38                 :            : 
      39                 :            : namespace autoware
      40                 :            : {
      41                 :            : namespace common
      42                 :            : {
      43                 :            : namespace geometry
      44                 :            : {
      45                 :            : /// \brief All objects related to the spatial hash data structure for efficient near neighbor lookup
      46                 :            : namespace spatial_hash
      47                 :            : {
      48                 :            : /// \brief Index type for identifying bins of the hash/lattice
      49                 :            : using Index = std::size_t;
      50                 :            : /// \brief Spatial hash functionality not intended to be used by an external user
      51                 :            : namespace details
      52                 :            : {
      53                 :            : /// \brief Internal struct for packing three indices together
      54                 :            : ///
      55                 :            : /// The use of this struct publically is a violation of our coding standards, but I claim it's
      56                 :            : /// fine because (a) it's details, (b) it is literally three unrelated members packaged together.
      57                 :            : /// This type is needed for conceptual convenience so I don't have massive function parameter
      58                 :            : /// lists
      59                 :            : struct GEOMETRY_PUBLIC Index3
      60                 :            : {
      61                 :            :   Index x;
      62                 :            :   Index y;
      63                 :            :   Index z;
      64                 :            : };  // struct Index3
      65                 :            : 
      66                 :            : using BinRange = std::pair<Index3, Index3>;
      67                 :            : }  // namespace details
      68                 :            : 
      69                 :            : /// \brief The base class for the configuration object for the SpatialHash class
      70                 :            : /// \tparam Derived The type of the derived class to support static polymorphism/CRTP
      71                 :            : template<typename Derived>
      72                 :            : class GEOMETRY_PUBLIC Config : public autoware::common::helper_functions::crtp<Derived>
      73                 :            : {
      74                 :            : public:
      75                 :            :   /// \brief Constructor for spatial hash
      76                 :            :   /// \param[in] min_x The minimum x value for the spatial hash
      77                 :            :   /// \param[in] max_x The maximum x value for the spatial hash
      78                 :            :   /// \param[in] min_y The minimum y value for the spatial hash
      79                 :            :   /// \param[in] max_y The maximum y value for the spatial hash
      80                 :            :   /// \param[in] min_z The minimum z value for the spatial hash
      81                 :            :   /// \param[in] max_z The maximum z value for the spatial hash
      82                 :            :   /// \param[in] radius The look up radius
      83                 :            :   /// \param[in] capacity The maximum number of points the spatial hash can store
      84                 :         25 :   Config(
      85                 :            :     const float32_t min_x,
      86                 :            :     const float32_t max_x,
      87                 :            :     const float32_t min_y,
      88                 :            :     const float32_t max_y,
      89                 :            :     const float32_t min_z,
      90                 :            :     const float32_t max_z,
      91                 :            :     const float32_t radius,
      92                 :            :     const Index capacity)
      93                 :            :   : m_min_x{min_x},
      94                 :            :     m_min_y{min_y},
      95                 :            :     m_min_z{min_z},
      96                 :            :     m_max_x{max_x},
      97                 :            :     m_max_y{max_y},
      98                 :            :     m_max_z{max_z},
      99                 :            :     m_side_length{radius},
     100                 :         25 :     m_side_length2{radius * radius},
     101                 :         25 :     m_side_length_inv{1.0F / radius},
     102                 :            :     m_capacity{capacity},
     103                 :            :     m_max_x_idx{check_basis_direction(min_x, max_x)},
     104                 :            :     m_max_y_idx{check_basis_direction(min_y, max_y)},
     105                 :            :     m_max_z_idx{check_basis_direction(min_z, max_z)},
     106                 :         19 :     m_y_stride{m_max_x_idx + 1U},
     107                 :         25 :     m_z_stride{m_y_stride * (m_max_y_idx + 1U)}
     108                 :            :   {
     109         [ +  + ]:         19 :     if (radius <= 0.0F) {
     110         [ +  - ]:          1 :       throw std::domain_error("Error constructing SpatialHash: must have positive side length");
     111                 :            :     }
     112                 :            : 
     113         [ -  + ]:         18 :     if ((m_max_y_idx + m_y_stride) > std::numeric_limits<Index>::max() / 2U) {
     114                 :            :       // TODO(c.ho) properly check for multiplication overflow
     115         [ #  # ]:          0 :       throw std::domain_error("SpatialHash::Config: voxel index may overflow!");
     116                 :            :     }
     117                 :            :     // small fudging to prevent weird boundary effects
     118                 :            :     // (e.g (x=xmax, y) rolls index over to (x=0, y+1)
     119                 :            :     constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
     120                 :            :     //lint -e{1938} read only access is fine NOLINT
     121                 :         18 :     m_max_x -= FEPS;
     122                 :         18 :     m_max_y -= FEPS;
     123                 :         18 :     m_max_z -= FEPS;
     124         [ -  + ]:         18 :     if ((m_z_stride + m_max_z_idx) > std::numeric_limits<Index>::max() / 2U) {
     125                 :            :       // TODO(c.ho) properly check for multiplication overflow
     126         [ #  # ]:          0 :       throw std::domain_error("SpatialHash::Config: voxel index may overflow!");
     127                 :            :     }
     128                 :            :     // I don't know if this is even possible given other checks
     129         [ -  + ]:         18 :     if (std::numeric_limits<Index>::max() == m_max_z_idx) {
     130         [ #  # ]:          0 :       throw std::domain_error("SpatialHash::Config: max z index exceeds reasonable value");
     131                 :            :     }
     132                 :         18 :   }
     133                 :            : 
     134                 :            :   /// \brief Given a reference index triple, compute the first and last bin
     135                 :            :   /// \param[in] ref The reference index triple
     136                 :            :   /// \return A pair where the first element is an index triple of the first bin, and the second
     137                 :            :   ///         element is an index triple for the last bin
     138                 :          3 :   details::BinRange bin_range(const details::Index3 & ref) const
     139                 :            :   {
     140                 :            :     // Compute distance in units of voxels
     141                 :          3 :     constexpr Index iradius = 1;
     142                 :            :     // Dumb ternary because potentially unsigned Index type
     143                 :          3 :     const Index xmin = (ref.x > iradius) ? (ref.x - iradius) : 0U;
     144                 :          3 :     const Index ymin = (ref.y > iradius) ? (ref.y - iradius) : 0U;
     145                 :          3 :     const Index zmin = (ref.z > iradius) ? (ref.z - iradius) : 0U;
     146                 :            :     // In 2D mode, m_max_z should be 0, same with ref.z
     147         [ #  # ]:          3 :     const Index xmax = std::min(ref.x + iradius, m_max_x_idx);
     148         [ #  # ]:          3 :     const Index ymax = std::min(ref.y + iradius, m_max_y_idx);
     149         [ #  # ]:          3 :     const Index zmax = std::min(ref.z + iradius, m_max_z_idx);
     150                 :            :     // return bottom-left portion of cube and top-right portion of cube
     151                 :          3 :     return {{xmin, ymin, zmin}, {xmax, ymax, zmax}};
     152                 :            :   }
     153                 :            :   /// \brief Get next index within a given range
     154                 :            :   /// \return True if idx is valid and still in range
     155                 :            :   /// \param[in] range The max and min bin indices
     156                 :            :   /// \param[inout] idx The index to be incremented, updated even if a negative result occurs
     157                 :         37 :   bool8_t next_bin(const details::BinRange & range, details::Index3 & idx) const
     158                 :            :   {
     159                 :            :     // TODO(c.ho) is there any way to make this neater without triple nested if?
     160                 :         37 :     bool8_t ret = true;
     161                 :         37 :     ++idx.x;
     162   [ +  +  #  # ]:         37 :     if (idx.x > range.second.x) {
     163                 :         13 :       idx.x = range.first.x;
     164                 :         13 :       ++idx.y;
     165   [ +  +  #  # ]:         13 :       if (idx.y > range.second.y) {
     166                 :          5 :         idx.y = range.first.y;
     167                 :          5 :         ++idx.z;
     168   [ +  +  #  # ]:          5 :         if (idx.z > range.second.z) {
     169                 :          3 :           ret = false;
     170                 :            :         }
     171                 :            :       }
     172                 :            :     }
     173                 :         37 :     return ret;
     174                 :            :   }
     175                 :            :   /// \brief Get the maximum capacity of the spatial hash
     176                 :            :   /// \return The capacity
     177                 :         27 :   Index get_capacity() const
     178                 :            :   {
     179                 :         27 :     return m_capacity;
     180                 :            :   }
     181                 :            : 
     182                 :            :   /// \brief Getter for the side length, equivalently the lookup radius
     183                 :        182 :   float32_t radius2() const
     184                 :            :   {
     185                 :        182 :     return m_side_length2;
     186                 :            :   }
     187                 :            : 
     188                 :            :   ////////////////////////////////////////////////////////////////////////////////////////////
     189                 :            :   // "Polymorphic" API
     190                 :            :   /// \brief Compute the single index given a point
     191                 :            :   /// \param[in] x The x component of the point
     192                 :            :   /// \param[in] y The y component of the point
     193                 :            :   /// \param[in] z The z component of the point
     194                 :            :   /// \return The combined index of the bin for a given point
     195                 :        182 :   Index bin(const float32_t x, const float32_t y, const float32_t z) const
     196                 :            :   {
     197                 :        182 :     return this->impl().bin_(x, y, z);
     198                 :            :   }
     199                 :            :   /// \brief Compute whether the query bin and reference bin could possibly contain a pair of points
     200                 :            :   ///        such that their distance is within a certain threshold
     201                 :            :   /// \param[in] ref The index triple of the bin containing the reference point
     202                 :            :   /// \param[in] query The index triple of the bin being queried
     203                 :            :   /// \return True if query and ref could possibly hold points within reference distance to one
     204                 :            :   ///         another
     205                 :         37 :   bool8_t is_candidate_bin(const details::Index3 & ref, const details::Index3 & query) const
     206                 :            :   {
     207                 :         37 :     return this->impl().valid(ref, query);
     208                 :            :   }
     209                 :            :   /// \brief Compute the decomposed index given a point
     210                 :            :   /// \param[in] x The x component of the point
     211                 :            :   /// \param[in] y The y component of the point
     212                 :            :   /// \param[in] z The z component of the point
     213                 :            :   /// \return The decomposed index triple of the bin for the given point
     214                 :          3 :   details::Index3 index3(const float32_t x, const float32_t y, const float32_t z) const
     215                 :            :   {
     216                 :          3 :     return this->impl().index3_(x, y, z);
     217                 :            :   }
     218                 :            :   /// \brief Compute the composed single index given a decomposed index
     219                 :            :   /// \param[in] idx A decomposed index triple for a bin
     220                 :            :   /// \return The composed bin index
     221                 :         37 :   Index index(const details::Index3 & idx) const
     222                 :            :   {
     223                 :         37 :     return this->impl().index_(idx);
     224                 :            :   }
     225                 :            :   /// \brief Compute the squared distance between the two points
     226                 :            :   /// \tparam PointT A point type with float members x, y and z, or point adapters defined
     227                 :            :   /// \param[in] x The x component of the first point
     228                 :            :   /// \param[in] y The y component of the first point
     229                 :            :   /// \param[in] z The z component of the first point
     230                 :            :   /// \param[in] pt The other point being compared
     231                 :            :   /// \return The squared distance between the points (2d or 3d)
     232                 :            :   template<typename PointT>
     233                 :        182 :   float32_t distance_squared(
     234                 :            :     const float32_t x,
     235                 :            :     const float32_t y,
     236                 :            :     const float32_t z,
     237                 :            :     const PointT & pt) const
     238                 :            :   {
     239                 :        182 :     return this->impl().distance_squared_(x, y, z, pt);
     240                 :            :   }
     241                 :            : 
     242                 :            : protected:
     243                 :            :   /// \brief Computes the index in the x basis direction
     244                 :            :   /// \param[in] x The x value of a point
     245                 :            :   /// \return The x offset of the index
     246                 :       1479 :   Index x_index(const float32_t x) const
     247                 :            :   {
     248                 :            :     return static_cast<Index>(
     249         [ +  + ]:       1479 :       std::floor((std::min(std::max(x, m_min_x), m_max_x) - m_min_x) * m_side_length_inv));
     250                 :            :   }
     251                 :            :   /// \brief Computes the index in the y basis direction
     252                 :            :   /// \param[in] y The y value of a point
     253                 :            :   /// \return The x offset of the index
     254                 :       1479 :   Index y_index(const float32_t y) const
     255                 :            :   {
     256                 :            :     return static_cast<Index>(
     257         [ +  + ]:       1479 :       std::floor((std::min(std::max(y, m_min_y), m_max_y) - m_min_y) * m_side_length_inv));
     258                 :            :   }
     259                 :            :   /// \brief Computes the index in the z basis direction
     260                 :            :   /// \param[in] z The z value of a point
     261                 :            :   /// \return The x offset of the index
     262                 :        161 :   Index z_index(const float32_t z) const
     263                 :            :   {
     264                 :            :     return static_cast<Index>(
     265         [ -  + ]:        161 :       std::floor((std::min(std::max(z, m_min_z), m_max_z) - m_min_z) * m_side_length_inv));
     266                 :            :   }
     267                 :            :   /// \brief Compose the provided index offsets
     268                 :            :   Index bin_impl(const Index xdx, const Index ydx) const
     269                 :            :   {
     270                 :       6689 :     return xdx + (ydx * m_y_stride);
     271                 :            :   }
     272                 :            :   /// \brief Compose the provided index offsets
     273                 :            :   Index bin_impl(const Index xdx, const Index ydx, const Index zdx) const
     274                 :            :   {
     275                 :         27 :     return bin_impl(xdx, ydx) + (zdx * m_z_stride);
     276                 :            :   }
     277                 :            : 
     278                 :            :   /// \brief The index offset of a point given it's x and y values
     279                 :            :   /// \param[in] x The x value of a point
     280                 :            :   /// \param[in] y the y value of a point
     281                 :            :   /// \return The index of the point in the 2D case, or the offset within a z-slice of the hash
     282                 :        829 :   Index bin_impl(const float32_t x, const float32_t y) const
     283                 :            :   {
     284                 :        829 :     return bin_impl(x_index(x), y_index(y));
     285                 :            :   }
     286                 :            :   /// \brief The index of a point given it's x, y and z values
     287                 :            :   /// \param[in] x The x value of a point
     288                 :            :   /// \param[in] y the y value of a point
     289                 :            :   /// \param[in] z the z value of a point
     290                 :            :   /// \return The index of the bin for the specified point
     291                 :        160 :   Index bin_impl(const float32_t x, const float32_t y, const float32_t z) const
     292                 :            :   {
     293                 :        160 :     return bin_impl(x, y) + (z_index(z) * m_z_stride);
     294                 :            :   }
     295                 :            :   /// \brief The distance between two indices as a float, where adjacent indices have zero
     296                 :            :   ///        distance (e.g. dist(0, 1) = 0)
     297                 :            :   float32_t idx_distance(const Index ref_idx, const Index query_idx) const
     298                 :            :   {
     299                 :            :     /// Not using fabs because Index is (possibly) unsigned
     300   [ +  +  +  +  :      11747 :     const Index idist = (ref_idx >= query_idx) ? (ref_idx - query_idx) : (query_idx - ref_idx);
          +  +  +  +  +  
                      + ]
     301                 :      11747 :     float32_t dist = static_cast<float32_t>(idist) - 1.0F;
     302   [ +  +  +  +  :      13710 :     return std::max(dist, 0.0F);
          +  +  +  +  +  
          +  +  +  +  +  
                   +  + ]
     303                 :            :   }
     304                 :            : 
     305                 :            : private:
     306                 :            :   /// \brief Sanity check a range in a basis direction
     307                 :            :   /// \return The number of voxels in the given basis direction
     308                 :         69 :   Index check_basis_direction(const float32_t min, const float32_t max) const
     309                 :            :   {
     310         [ +  + ]:         69 :     if (min >= max) {
     311         [ +  - ]:          3 :       throw std::domain_error("SpatialHash::Config: must have min < max");
     312                 :            :     }
     313                 :            :     // This family of checks is to ensure that you don't get weird casting effects due to huge
     314                 :            :     // floating point values
     315                 :         66 :     const float64_t dmax = static_cast<float64_t>(max);
     316                 :         66 :     const float64_t dmin = static_cast<float64_t>(min);
     317                 :         66 :     const float64_t width = (dmax - dmin) * static_cast<float64_t>(m_side_length_inv);
     318                 :            :     constexpr float64_t fltmax = static_cast<float64_t>(std::numeric_limits<float32_t>::max());
     319         [ +  + ]:         66 :     if (fltmax <= width) {
     320         [ +  - ]:          3 :       throw std::domain_error("SpatialHash::Config: voxel size approaching floating point limit");
     321                 :            :     }
     322                 :         63 :     return static_cast<Index>(width);
     323                 :            :   }
     324                 :            :   float32_t m_min_x;
     325                 :            :   float32_t m_min_y;
     326                 :            :   float32_t m_min_z;
     327                 :            :   float32_t m_max_x;
     328                 :            :   float32_t m_max_y;
     329                 :            :   float32_t m_max_z;
     330                 :            :   float32_t m_side_length;
     331                 :            :   float32_t m_side_length2;
     332                 :            :   float32_t m_side_length_inv;
     333                 :            :   Index m_capacity;
     334                 :            :   Index m_max_x_idx;
     335                 :            :   Index m_max_y_idx;
     336                 :            :   Index m_max_z_idx;
     337                 :            :   Index m_y_stride;
     338                 :            :   Index m_z_stride;
     339                 :            : };  // class Config
     340                 :            : 
     341                 :            : /// \brief Configuration class for a 2d spatial hash
     342                 :            : class GEOMETRY_PUBLIC Config2d : public Config<Config2d>
     343                 :            : {
     344                 :            : public:
     345                 :            :   /// \brief Config constructor for 2D spatial hash
     346                 :            :   /// \param[in] min_x The minimum x value for the spatial hash
     347                 :            :   /// \param[in] max_x The maximum x value for the spatial hash
     348                 :            :   /// \param[in] min_y The minimum y value for the spatial hash
     349                 :            :   /// \param[in] max_y The maximum y value for the spatial hash
     350                 :            :   /// \param[in] radius The lookup distance
     351                 :            :   /// \param[in] capacity The maximum number of points the spatial hash can store
     352                 :            :   Config2d(
     353                 :            :     const float32_t min_x,
     354                 :            :     const float32_t max_x,
     355                 :            :     const float32_t min_y,
     356                 :            :     const float32_t max_y,
     357                 :            :     const float32_t radius,
     358                 :            :     const Index capacity);
     359                 :            :   /// \brief The index of a point given it's x, y and z values, 2d implementation
     360                 :            :   /// \param[in] x The x value of a point
     361                 :            :   /// \param[in] y the y value of a point
     362                 :            :   /// \param[in] z the z value of a point
     363                 :            :   /// \return The index of the bin for the specified point
     364                 :            :   Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
     365                 :            :   /// \brief Determine if a bin could possibly hold a point within a distance to any point in a
     366                 :            :   ///        reference bin for the 2D case
     367                 :            :   /// \param[in] ref The decomposed index triple of the reference bin
     368                 :            :   /// \param[in] query The decomposed index triple of the bin being queried
     369                 :            :   /// \return True if the reference bin and query bin could possibly hold a point within the
     370                 :            :   ///         reference distance
     371                 :            :   bool8_t valid(const details::Index3 & ref, const details::Index3 & query) const;
     372                 :            :   /// \brief Compute the decomposed index given a point, 2d implementation
     373                 :            :   /// \param[in] x The x component of the point
     374                 :            :   /// \param[in] y The y component of the point
     375                 :            :   /// \param[in] z The z component of the point
     376                 :            :   /// \return The decomposed index triple of the bin for the given point
     377                 :            :   details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
     378                 :            :   /// \brief Compute the composed single index given a decomposed index, 2d implementation
     379                 :            :   /// \param[in] idx A decomposed index triple for a bin
     380                 :            :   /// \return The composed bin index
     381                 :            :   Index index_(const details::Index3 & idx) const;
     382                 :            :   /// \brief Compute the squared distance between the two points, 2d implementation
     383                 :            :   /// \tparam PointT A point type with float members x, y and z, or point adapters defined
     384                 :            :   /// \param[in] x The x component of the first point
     385                 :            :   /// \param[in] y The y component of the first point
     386                 :            :   /// \param[in] z The z component of the first point
     387                 :            :   /// \param[in] pt The other point being compared
     388                 :            :   /// \return The squared distance between the points (2d)
     389                 :            :   template<typename PointT>
     390                 :         22 :   float32_t distance_squared_(
     391                 :            :     const float32_t x,
     392                 :            :     const float32_t y,
     393                 :            :     const float32_t z,
     394                 :            :     const PointT & pt) const
     395                 :            :   {
     396                 :            :     (void)z;
     397                 :         22 :     const float32_t dx = x - point_adapter::x_(pt);
     398                 :         22 :     const float32_t dy = y - point_adapter::y_(pt);
     399         [ #  # ]:         22 :     return (dx * dx) + (dy * dy);
     400                 :            :   }
     401                 :            : };  // class Config2d
     402                 :            : 
     403                 :            : /// \brief Configuration class for a 3d spatial hash
     404                 :            : class GEOMETRY_PUBLIC Config3d : public Config<Config3d>
     405                 :            : {
     406                 :            : public:
     407                 :            :   /// \brief Config constructor for a 3d spatial hash
     408                 :            :   /// \param[in] min_x The minimum x value for the spatial hash
     409                 :            :   /// \param[in] max_x The maximum x value for the spatial hash
     410                 :            :   /// \param[in] min_y The minimum y value for the spatial hash
     411                 :            :   /// \param[in] max_y The maximum y value for the spatial hash
     412                 :            :   /// \param[in] min_z The minimum z value for the spatial hash
     413                 :            :   /// \param[in] max_z The maximum z value for the spatial hash
     414                 :            :   /// \param[in] radius The lookup distance
     415                 :            :   /// \param[in] capacity The maximum number of points the spatial hash can store
     416                 :            :   Config3d(
     417                 :            :     const float32_t min_x,
     418                 :            :     const float32_t max_x,
     419                 :            :     const float32_t min_y,
     420                 :            :     const float32_t max_y,
     421                 :            :     const float32_t min_z,
     422                 :            :     const float32_t max_z,
     423                 :            :     const float32_t radius,
     424                 :            :     const Index capacity);
     425                 :            :   /// \brief The index of a point given it's x, y and z values, 3d implementation
     426                 :            :   /// \param[in] x The x value of a point
     427                 :            :   /// \param[in] y the y value of a point
     428                 :            :   /// \param[in] z the z value of a point
     429                 :            :   /// \return The index of the bin for the specified point
     430                 :            :   Index bin_(const float32_t x, const float32_t y, const float32_t z) const;
     431                 :            :   /// \brief Determine if a bin could possibly hold a point within a distance to any point in a
     432                 :            :   ///        reference bin for the 3D case
     433                 :            :   /// \param[in] ref The decomposed index triple of the reference bin
     434                 :            :   /// \param[in] query The decomposed index triple of the bin being queried
     435                 :            :   /// \return True if the reference bin and query bin could possibly hold a point within the
     436                 :            :   ///         reference distance
     437                 :            :   bool8_t valid(const details::Index3 & ref, const details::Index3 & query) const;
     438                 :            :   /// \brief Compute the decomposed index given a point, 3d implementation
     439                 :            :   /// \param[in] x The x component of the point
     440                 :            :   /// \param[in] y The y component of the point
     441                 :            :   /// \param[in] z The z component of the point
     442                 :            :   /// \return The decomposed index triple of the bin for the given point
     443                 :            :   details::Index3 index3_(const float32_t x, const float32_t y, const float32_t z) const;
     444                 :            :   /// \brief Compute the composed single index given a decomposed index, 3d implementation
     445                 :            :   /// \param[in] idx A decomposed index triple for a bin
     446                 :            :   /// \return The composed bin index
     447                 :            :   Index index_(const details::Index3 & idx) const;
     448                 :            :   /// \brief Compute the squared distance between the two points, 3d implementation
     449                 :            :   /// \tparam PointT A point type with float members x, y and z, or point adapters defined
     450                 :            :   /// \param[in] x The x component of the first point
     451                 :            :   /// \param[in] y The y component of the first point
     452                 :            :   /// \param[in] z The z component of the first point
     453                 :            :   /// \param[in] pt The other point being compared
     454                 :            :   /// \return The squared distance between the points (3d)
     455                 :            :   template<typename PointT>
     456                 :        160 :   float32_t distance_squared_(
     457                 :            :     const float32_t x,
     458                 :            :     const float32_t y,
     459                 :            :     const float32_t z,
     460                 :            :     const PointT & pt) const
     461                 :            :   {
     462                 :        160 :     const float32_t dx = x - point_adapter::x_(pt);
     463                 :        160 :     const float32_t dy = y - point_adapter::y_(pt);
     464                 :        160 :     const float32_t dz = z - point_adapter::z_(pt);
     465         [ #  # ]:        160 :     return (dx * dx) + (dy * dy) + (dz * dz);
     466                 :            :   }
     467                 :            : };  // class Config3d
     468                 :            : }  // namespace spatial_hash
     469                 :            : }  // namespace geometry
     470                 :            : }  // namespace common
     471                 :            : }  // namespace autoware
     472                 :            : 
     473                 :            : #endif  // GEOMETRY__SPATIAL_HASH_CONFIG_HPP_

Generated by: LCOV version 1.14