LCOV - code coverage report
Current view: top level - src/perception/filters/voxel_grid/include/voxel_grid - config.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 28 28 100.0 %
Date: 2023-03-03 05:44:19 Functions: 4 4 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 22 22 100.0 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2017-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                 :            : 
      17                 :            : /// \file
      18                 :            : /// \brief This file defines the configuration class for the voxel grid data structure
      19                 :            : 
      20                 :            : #ifndef VOXEL_GRID__CONFIG_HPP_
      21                 :            : #define VOXEL_GRID__CONFIG_HPP_
      22                 :            : 
      23                 :            : #include <voxel_grid/visibility_control.hpp>
      24                 :            : #include <geometry_msgs/msg/point32.hpp>
      25                 :            : #include <geometry/common_2d.hpp>
      26                 :            : #include <common/types.hpp>
      27                 :            : #include <cmath>
      28                 :            : 
      29                 :            : using autoware::common::types::float32_t;
      30                 :            : 
      31                 :            : namespace autoware
      32                 :            : {
      33                 :            : namespace perception
      34                 :            : {
      35                 :            : namespace filters
      36                 :            : {
      37                 :            : namespace voxel_grid
      38                 :            : {
      39                 :            : // TODO(cvasfi) move this definition to a shared/common place
      40                 :            : 
      41                 :            : template<typename T>
      42                 :            : inline T clamp(const T val, const T min, const T max)
      43                 :            : {
      44   [ +  +  +  +  :   15264476 :   return (val < min) ? min : ((val > max) ? max : val);
          +  +  +  +  +  
                +  +  + ]
      45                 :            : }
      46                 :            : 
      47                 :            : using PointXYZ = geometry_msgs::msg::Point32;
      48                 :            : 
      49                 :            : 
      50                 :            : /// \brief A configuration class for the VoxelGrid data structure, also includes some helper
      51                 :            : ///        functionality for computing indices and centroids of voxels.
      52                 :            : class VOXEL_GRID_PUBLIC Config
      53                 :            : {
      54                 :            : public:
      55                 :            :   static constexpr float32_t MIN_VOXEL_SIZE_M = 0.01F;
      56                 :            :   /// \brief Constructor
      57                 :            :   /// \param[in] min_point The minimum corner of the receptive field (box) for the voxel grid
      58                 :            :   /// \param[in] max_point The maximum corner of the receptive field (box) for the voxel grid
      59                 :            :   /// \param[in] voxel_size The size of each voxel
      60                 :            :   /// \param[in] capacity The preallocated size of the voxel grid
      61                 :            :   Config(
      62                 :            :     const PointXYZ & min_point,
      63                 :            :     const PointXYZ & max_point,
      64                 :            :     const PointXYZ & voxel_size,
      65                 :            :     const uint64_t capacity);
      66                 :            :   /// \brief Gets the minimum corner of the voxel grid
      67                 :            :   /// \return Fixed value
      68                 :            :   const PointXYZ & get_min_point() const;
      69                 :            :   /// \brief Gets the maximum corner of the voxel grid
      70                 :            :   /// \return Fixed value
      71                 :            :   const PointXYZ & get_max_point() const;
      72                 :            :   /// \brief Gets the voxel size of the voxel grid
      73                 :            :   /// \return Fixed value
      74                 :            :   const PointXYZ & get_voxel_size() const;
      75                 :            :   /// \brief Gets the capacity of the voxel grid
      76                 :            :   /// \return Fixed value
      77                 :            :   uint64_t get_capacity() const;
      78                 :            :   /// \brief Computes index for a given point given the voxelgrid configuration parameters
      79                 :            :   /// \param[in] pt The point for which the voxel index will be computed
      80                 :            :   /// \return The index of the voxel for which this point will fall into
      81                 :            :   /// \tparam PointT The point type taken. Assumed to have public float32_t members x, y, and z.
      82                 :            :   ///                Also assumed to have a default constructor.
      83                 :            :   template<typename PointT>
      84                 :    5088832 :   uint64_t index(const PointT & pt) const
      85                 :            :   {
      86                 :    5088832 :     const uint64_t idx = static_cast<uint64_t>(
      87                 :    5088832 :       std::floor(
      88                 :    5088832 :         (clamp(
      89                 :            :           common::geometry::point_adapter::x_(pt),
      90   [ +  +  +  + ]:   10177680 :           m_min_point.x, m_max_point.x) - m_min_point.x) * m_voxel_size_inv.x));
      91                 :    5088832 :     const uint64_t jdx = static_cast<uint64_t>(
      92                 :    5088832 :       std::floor(
      93                 :    5088832 :         (clamp(
      94                 :            :           common::geometry::point_adapter::y_(pt),
      95   [ +  +  +  + ]:   10177680 :           m_min_point.y, m_max_point.y) - m_min_point.y) * m_voxel_size_inv.y));
      96                 :    5088832 :     const uint64_t kdx = static_cast<uint64_t>(
      97                 :    5088832 :       std::floor(
      98                 :    5088832 :         (clamp(
      99                 :            :           common::geometry::point_adapter::z_(pt),
     100         [ +  + ]:    5088832 :           m_min_point.z, m_max_point.z) - m_min_point.z) * m_voxel_size_inv.z));
     101                 :    5088832 :     return idx + (jdx * m_y_stride) + (kdx * m_z_stride);
     102                 :            :   }
     103                 :            : 
     104                 :            :   /// \brief Computes the centroid for a given voxel index
     105                 :            :   /// \param[in] index The index for a given voxel
     106                 :            :   /// \return A point for whom the x, y and z fields are filled out
     107                 :            :   /// \tparam PointT The point type returned. Assumed to have public float32_t members x, y, and z.
     108                 :            :   ///                Also assumed to have a default constructor.
     109                 :            :   ///                Only the x, y, and z fields will be filled out.
     110                 :            :   template<typename PointT>
     111                 :         31 :   PointT centroid(const uint64_t index) const
     112                 :            :   {
     113                 :            :     // 'deserialize' indices
     114                 :         31 :     const uint64_t zdx = index / m_z_stride;
     115                 :         31 :     const uint64_t jdx = (index % m_z_stride);
     116                 :         31 :     const uint64_t ydx = jdx / m_y_stride;
     117                 :         31 :     const uint64_t xdx = jdx % m_y_stride;
     118                 :            :     // compute centroid of voxel
     119                 :         10 :     PointT pt;
     120                 :            : 
     121                 :         10 :     common::geometry::point_adapter::xr_(pt) =
     122                 :         31 :       ((static_cast<float32_t>(xdx) + 0.5F) * m_voxel_size.x) + m_min_point.x;
     123                 :         10 :     common::geometry::point_adapter::yr_(pt) =
     124                 :         31 :       ((static_cast<float32_t>(ydx) + 0.5F) * m_voxel_size.y) + m_min_point.y;
     125                 :         10 :     common::geometry::point_adapter::zr_(pt) =
     126                 :         31 :       ((static_cast<float32_t>(zdx) + 0.5F) * m_voxel_size.z) + m_min_point.z;
     127                 :         31 :     return pt;
     128                 :            :   }
     129                 :            : 
     130                 :            : private:
     131                 :            :   /// \brief Sanity check a range in a basis direction
     132                 :            :   /// \return The number of voxels in the given basis direction (aka width in units of voxels)
     133                 :            :   /// \param[in] min The lower bound in the specified basis direction
     134                 :            :   /// \param[in] max The upper bound in the specified basis direction
     135                 :            :   /// \param[in] size The voxel size in the specified basis direction
     136                 :            :   uint64_t check_basis_direction(
     137                 :            :     const float32_t min,
     138                 :            :     const float32_t max,
     139                 :            :     const float32_t size) const;
     140                 :            : 
     141                 :            :   PointXYZ m_min_point;
     142                 :            :   PointXYZ m_max_point;
     143                 :            :   PointXYZ m_voxel_size;
     144                 :            :   PointXYZ m_voxel_size_inv;
     145                 :            :   uint64_t m_y_stride;
     146                 :            :   uint64_t m_z_stride;
     147                 :            :   uint64_t m_capacity;
     148                 :            : };  // class Config
     149                 :            : }  // namespace voxel_grid
     150                 :            : }  // namespace filters
     151                 :            : }  // namespace perception
     152                 :            : }  // namespace autoware
     153                 :            : 
     154                 :            : #endif  // VOXEL_GRID__CONFIG_HPP_

Generated by: LCOV version 1.14