Go to the documentation of this file.
20 #ifndef VOXEL_GRID__VOXEL_GRID_HPP_
21 #define VOXEL_GRID__VOXEL_GRID_HPP_
26 #include <forward_list>
27 #include <unordered_map>
43 template<
typename VoxelT>
46 using Grid = std::unordered_map<uint64_t, VoxelT>;
47 using IT =
typename Grid::const_iterator;
48 using OutputQueue = std::forward_list<IT>;
56 m_map(m_config.get_capacity()),
57 m_output_pool(m_config.get_capacity()),
58 m_output(m_output_pool.get_allocator()),
59 m_new_voxels_called(false),
60 m_last_output_begin(m_output.
end())
70 const uint64_t idx = m_config.index(pt);
72 if (m_map.end() == m_map.find(idx)) {
73 if (capacity() <= size()) {
74 throw std::length_error{
"VoxelGrid: insertion would overrun capacity"};
78 VoxelT & vx = m_map[idx];
87 vx.configure(m_config, idx);
90 const auto it = m_output_pool.begin();
91 *it = m_map.find(idx);
92 m_output.splice_after(m_output.cbefore_begin(), m_output_pool, m_output_pool.before_begin());
93 if (m_new_voxels_called) {
94 m_last_output_begin = m_output.begin();
95 m_new_voxels_called =
false;
99 vx.add_observation(pt);
105 template<
typename IT>
108 for (IT it = begin; it !=
end; ++it) {
127 m_output_pool.splice_after(
128 m_output_pool.cbefore_begin(),
132 m_new_voxels_called =
true;
133 m_last_output_begin = m_output.before_begin();
147 return m_map.cbegin();
165 m_output_pool.splice_after(m_output_pool.before_begin(), m_output);
166 m_new_voxels_called =
false;
167 m_last_output_begin = m_output.end();
179 return m_config.get_capacity();
185 return m_map.empty();
192 OutputQueue m_output_pool;
193 OutputQueue m_output;
195 typename OutputQueue::iterator m_last_output_begin;
203 #endif // VOXEL_GRID__VOXEL_GRID_HPP_
typename autoware::perception::filters::voxel_grid::CentroidVoxel< autoware::common::types::PointXYZIF > ::point_t point_t
Definition: voxel_grid.hpp:64
This file defines children and specializations of the Voxel class.
std::size_t size() const
Returns the current size of the voxel grid.
Definition: voxel_grid.hpp:171
IT cbegin() const
Returns an iterator to the first element of the voxel grid.
Definition: voxel_grid.hpp:145
This file includes common type definition.
This file defines the configuration class for the voxel grid data structure.
IT cend() const
Returns an iterator to one past the last element of the voxel grid.
Definition: voxel_grid.hpp:157
IT end() const
Returns an iterator to one past the last element of the voxel grid.
Definition: voxel_grid.hpp:151
VoxelGrid(const Config &cfg)
Constructor.
Definition: voxel_grid.hpp:54
bool bool8_t
Definition: types.hpp:39
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
void insert(const point_t &pt)
Inserts a point into the voxel grid, may result in new voxels being activated.
Definition: voxel_grid.hpp:67
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
bool8_t empty() const
Whether the voxel grid is empty.
Definition: voxel_grid.hpp:183
const OutputQueue & new_voxels()
Return a list of iterators pointing to newly activated voxels. Voxels will be removed from queue duri...
Definition: voxel_grid.hpp:124
IT begin() const
Returns an iterator to the first element of the voxel grid.
Definition: voxel_grid.hpp:139
void clear()
Resets the state of the voxel grid.
Definition: voxel_grid.hpp:162
void insert(const IT begin, const IT end)
Inserts many points into the voxel grid, dispatches to the core insert method.
Definition: voxel_grid.hpp:106
end
Definition: scripts/get_open_port.py:23
std::size_t capacity() const
Returns the preallocated capacity of the voxel grid.
Definition: voxel_grid.hpp:177
A voxel grid data structure for downsampling point clouds.
Definition: voxel_grid.hpp:44