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_