Autoware.Auto
perception/filters/voxel_grid/include/voxel_grid/config.hpp
Go to the documentation of this file.
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 
19 
20 #ifndef VOXEL_GRID__CONFIG_HPP_
21 #define VOXEL_GRID__CONFIG_HPP_
22 
24 #include <geometry_msgs/msg/point32.hpp>
25 #include <geometry/common_2d.hpp>
26 #include <common/types.hpp>
27 #include <cmath>
28 
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  return (val < min) ? min : ((val > max) ? max : val);
45 }
46 
47 using PointXYZ = geometry_msgs::msg::Point32;
48 
49 
52 class VOXEL_GRID_PUBLIC Config
53 {
54 public:
55  static constexpr float32_t MIN_VOXEL_SIZE_M = 0.01F;
61  Config(
62  const PointXYZ & min_point,
63  const PointXYZ & max_point,
64  const PointXYZ & voxel_size,
65  const uint64_t capacity);
68  const PointXYZ & get_min_point() const;
71  const PointXYZ & get_max_point() const;
74  const PointXYZ & get_voxel_size() const;
77  uint64_t get_capacity() const;
83  template<typename PointT>
84  uint64_t index(const PointT & pt) const
85  {
86  const uint64_t idx = static_cast<uint64_t>(
87  std::floor(
88  (clamp(
90  m_min_point.x, m_max_point.x) - m_min_point.x) * m_voxel_size_inv.x));
91  const uint64_t jdx = static_cast<uint64_t>(
92  std::floor(
93  (clamp(
95  m_min_point.y, m_max_point.y) - m_min_point.y) * m_voxel_size_inv.y));
96  const uint64_t kdx = static_cast<uint64_t>(
97  std::floor(
98  (clamp(
100  m_min_point.z, m_max_point.z) - m_min_point.z) * m_voxel_size_inv.z));
101  return idx + (jdx * m_y_stride) + (kdx * m_z_stride);
102  }
103 
110  template<typename PointT>
111  PointT centroid(const uint64_t index) const
112  {
113  // 'deserialize' indices
114  const uint64_t zdx = index / m_z_stride;
115  const uint64_t jdx = (index % m_z_stride);
116  const uint64_t ydx = jdx / m_y_stride;
117  const uint64_t xdx = jdx % m_y_stride;
118  // compute centroid of voxel
119  PointT pt;
120 
122  ((static_cast<float32_t>(xdx) + 0.5F) * m_voxel_size.x) + m_min_point.x;
124  ((static_cast<float32_t>(ydx) + 0.5F) * m_voxel_size.y) + m_min_point.y;
126  ((static_cast<float32_t>(zdx) + 0.5F) * m_voxel_size.z) + m_min_point.z;
127  return pt;
128  }
129 
130 private:
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_
autoware::common::geometry::point_adapter::x_
auto x_(const PointT &pt)
Gets the x value for a point.
Definition: common_2d.hpp:50
autoware::perception::filters::voxel_grid::clamp
T clamp(const T val, const T min, const T max)
Definition: perception/filters/voxel_grid/include/voxel_grid/config.hpp:42
types.hpp
This file includes common type definition.
visibility_control.hpp
common_2d.hpp
This file includes common functionality for 2D geometry, such as dot products.
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::common::geometry::point_adapter::yr_
auto & yr_(PointT &pt)
Gets a reference to the y value for a point.
Definition: common_2d.hpp:114
autoware::perception::filters::voxel_grid::Config::centroid
PointT centroid(const uint64_t index) const
Computes the centroid for a given voxel index.
Definition: perception/filters/voxel_grid/include/voxel_grid/config.hpp:111
autoware::common::types::float32_t
float float32_t
Definition: types.hpp:45
autoware::perception::filters::voxel_grid::Config
A configuration class for the VoxelGrid data structure, also includes some helper functionality for c...
Definition: perception/filters/voxel_grid/include/voxel_grid/config.hpp:52
autoware::common::geometry::point_adapter::xr_
auto & xr_(PointT &pt)
Gets a reference to the x value for a point.
Definition: common_2d.hpp:98
autoware::perception::filters::voxel_grid::PointXYZ
geometry_msgs::msg::Point32 PointXYZ
Definition: perception/filters/voxel_grid/include/voxel_grid/config.hpp:47
autoware::common::geometry::point_adapter::y_
auto y_(const PointT &pt)
Gets the y value for a point.
Definition: common_2d.hpp:66
autoware::common::geometry::point_adapter::z_
auto z_(const PointT &pt)
Gets the z value for a point.
Definition: common_2d.hpp:82
autoware::common::geometry::point_adapter::zr_
auto & zr_(PointT &pt)
Gets a reference to the z value for a point.
Definition: common_2d.hpp:130
autoware::perception::filters::voxel_grid::Config::index
uint64_t index(const PointT &pt) const
Computes index for a given point given the voxelgrid configuration parameters.
Definition: perception/filters/voxel_grid/include/voxel_grid/config.hpp:84