Go to the documentation of this file.
20 #ifndef VOXEL_GRID__CONFIG_HPP_
21 #define VOXEL_GRID__CONFIG_HPP_
24 #include <geometry_msgs/msg/point32.hpp>
42 inline T
clamp(
const T val,
const T min,
const T max)
44 return (val < min) ? min : ((val > max) ? max : val);
55 static constexpr
float32_t MIN_VOXEL_SIZE_M = 0.01F;
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 Po
intT>
84 uint64_t
index(
const PointT & pt)
const
86 const uint64_t idx =
static_cast<uint64_t
>(
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
>(
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
>(
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);
110 template<
typename Po
intT>
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;
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;
136 uint64_t check_basis_direction(
154 #endif // VOXEL_GRID__CONFIG_HPP_
auto x_(const PointT &pt)
Gets the x value for a point.
Definition: common_2d.hpp:50
T clamp(const T val, const T min, const T max)
Definition: perception/filters/voxel_grid/include/voxel_grid/config.hpp:42
This file includes common type definition.
This file includes common functionality for 2D geometry, such as dot products.
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
auto & yr_(PointT &pt)
Gets a reference to the y value for a point.
Definition: common_2d.hpp:114
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
float float32_t
Definition: types.hpp:45
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
auto & xr_(PointT &pt)
Gets a reference to the x value for a point.
Definition: common_2d.hpp:98
geometry_msgs::msg::Point32 PointXYZ
Definition: perception/filters/voxel_grid/include/voxel_grid/config.hpp:47
auto y_(const PointT &pt)
Gets the y value for a point.
Definition: common_2d.hpp:66
auto z_(const PointT &pt)
Gets the z value for a point.
Definition: common_2d.hpp:82
auto & zr_(PointT &pt)
Gets a reference to the z value for a point.
Definition: common_2d.hpp:130
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