Go to the documentation of this file.
17 #ifndef NDT__NDT_GRID_HPP_
18 #define NDT__NDT_GRID_HPP_
25 #include <unordered_map>
33 namespace localization
39 template<
typename VoxelT>
43 using Grid = std::unordered_map<uint64_t, VoxelT>;
52 : m_config(voxel_grid_config), m_map(m_config.get_capacity())
54 m_output_vector.reserve(1U);
85 m_output_vector.clear();
86 const auto vx_it = m_map.find(m_config.
index(pt));
88 if (vx_it != m_map.end() && vx_it->second.usable()) {
89 m_output_vector.emplace_back(vx_it->second);
91 return m_output_vector;
97 std::size_t
size() const noexcept
111 typename Grid::const_iterator
begin() const noexcept
118 typename Grid::iterator
begin() noexcept
120 return m_map.begin();
125 typename Grid::const_iterator
cbegin() const noexcept
127 return m_map.cbegin();
132 typename Grid::const_iterator
end() const noexcept
139 typename Grid::iterator
end() noexcept
146 typename Grid::const_iterator
cend() const noexcept
162 return m_config.
index(pt);
168 template<
typename ... Args>
171 return m_map.emplace(std::forward<Args>(args)...);
178 m_map[
index(pt)].add_observation(pt);
208 namespace point_adapter
214 inline NDT_PUBLIC
auto x_(
const Eigen::Vector3d & pt)
220 inline NDT_PUBLIC
auto y_(
const Eigen::Vector3d & pt)
226 inline NDT_PUBLIC
auto z_(
const Eigen::Vector3d & pt)
232 inline NDT_PUBLIC
auto &
xr_(Eigen::Vector3d & pt)
238 inline NDT_PUBLIC
auto &
yr_(Eigen::Vector3d & pt)
244 inline NDT_PUBLIC
auto &
zr_(Eigen::Vector3d & pt)
253 #endif // NDT__NDT_GRID_HPP_
Eigen::Vector3d Point
Definition: ndt_grid.hpp:44
auto emplace_voxel(Args &&... args)
Emplace a new voxel into the grid.
Definition: ndt_grid.hpp:169
void set_config(const Config &config)
Set the configuration.
Definition: ndt_grid.hpp:183
const PointXYZ & get_voxel_size() const
Gets the voxel size of the voxel grid.
Definition: perception/filters/voxel_grid/src/config.cpp:115
autoware::perception::filters::voxel_grid::Config Config
Definition: ndt_grid.hpp:45
Grid::const_iterator cbegin() const noexcept
Returns a const iterator to the first element of the map.
Definition: ndt_grid.hpp:125
auto x_(const PointT &pt)
Gets the x value for a point.
Definition: common_2d.hpp:50
auto index(const Point &pt) const
Definition: ndt_grid.hpp:160
This file includes common type definition.
DifferentialState y
Definition: kinematic_bicycle.snippet.hpp:28
const VoxelViewVector & cell(float32_t x, float32_t y, float32_t z) const
Definition: ndt_grid.hpp:73
NDTGrid(const Config &voxel_grid_config)
Definition: ndt_grid.hpp:51
A voxel grid implementation for normal distribution transform.
Definition: ndt_grid.hpp:40
const Config & config() const
Get the underlying voxel grid configuration.
Definition: ndt_grid.hpp:190
const PointXYZ & get_min_point() const
Gets the minimum corner of the voxel grid.
Definition: perception/filters/voxel_grid/src/config.cpp:105
void clear() noexcept
Clear all voxels in the map.
Definition: ndt_grid.hpp:152
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
NDTGrid & operator=(const NDTGrid &)=delete
auto & yr_(PointT &pt)
Gets a reference to the y value for a point.
Definition: common_2d.hpp:114
Grid::const_iterator begin() const noexcept
Returns an const iterator to the first element of the map.
Definition: ndt_grid.hpp:111
std::size_t size() const noexcept
Definition: ndt_grid.hpp:97
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
std::vector< VoxelView< autoware::localization::ndt::DynamicNDTVoxel > > VoxelViewVector
Definition: ndt_grid.hpp:46
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
std::unordered_map< uint64_t, autoware::localization::ndt::DynamicNDTVoxel > Grid
Definition: ndt_grid.hpp:43
std::decay_t< decltype(std::declval< Config >().get_min_point())> ConfigPoint
Definition: ndt_grid.hpp:47
const VoxelViewVector & cell(const Point &pt) const
Definition: ndt_grid.hpp:82
const ConfigPoint & cell_size() const noexcept
Definition: ndt_grid.hpp:104
auto y_(const PointT &pt)
Gets the y value for a point.
Definition: common_2d.hpp:66
Grid::const_iterator cend() const noexcept
Returns a const iterator to one past the last element of the map.
Definition: ndt_grid.hpp:146
void add_observation(const Point &pt)
Add a point to its corresponding voxel in the grid.
Definition: ndt_grid.hpp:176
auto z_(const PointT &pt)
Gets the z value for a point.
Definition: common_2d.hpp:82
Grid::const_iterator end() const noexcept
Returns a const iterator to one past the last element of the map.
Definition: ndt_grid.hpp:132
Grid::iterator end() noexcept
Returns an iterator to one past the last element of the map.
Definition: ndt_grid.hpp:139
auto & zr_(PointT &pt)
Gets a reference to the z value for a point.
Definition: common_2d.hpp:130
Grid::iterator begin() noexcept
Returns an iterator to the first element of the map.
Definition: ndt_grid.hpp:118
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