Autoware.Auto
ndt_grid.hpp
Go to the documentation of this file.
1 // Copyright 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 #ifndef NDT__NDT_GRID_HPP_
18 #define NDT__NDT_GRID_HPP_
19 
20 #include <common/types.hpp>
21 #include <ndt/ndt_common.hpp>
22 #include <ndt/ndt_voxel_view.hpp>
23 #include <vector>
24 #include <limits>
25 #include <unordered_map>
26 #include <utility>
27 #include <string>
28 
30 
31 namespace autoware
32 {
33 namespace localization
34 {
35 namespace ndt
36 {
39 template<typename VoxelT>
40 class NDTGrid
41 {
42 public:
43  using Grid = std::unordered_map<uint64_t, VoxelT>;
44  using Point = Eigen::Vector3d;
46  using VoxelViewVector = std::vector<VoxelView<VoxelT>>;
47  using ConfigPoint = std::decay_t<decltype(std::declval<Config>().get_min_point())>;
48 
51  explicit NDTGrid(const Config & voxel_grid_config)
52  : m_config(voxel_grid_config), m_map(m_config.get_capacity())
53  {
54  m_output_vector.reserve(1U);
55  }
56 
57  // Maps should be moved rather than being copied.
58  NDTGrid(const NDTGrid &) = delete;
59 
60  NDTGrid & operator=(const NDTGrid &) = delete;
61 
62  // Explicitly declaring to default is needed since we explicitly deleted the copy methods.
63  NDTGrid(NDTGrid &&) = default;
64 
65  NDTGrid & operator=(NDTGrid &&) = default;
66 
74  {
75  return cell(Point({x, y, z}));
76  }
77 
82  const VoxelViewVector & cell(const Point & pt) const
83  {
84  // TODO(yunus.caliskan): revisit after multi-cell lookup support. #985
85  m_output_vector.clear();
86  const auto vx_it = m_map.find(m_config.index(pt));
87  // Only return a voxel if it's occupied (i.e. has enough points to compute covariance.)
88  if (vx_it != m_map.end() && vx_it->second.usable()) {
89  m_output_vector.emplace_back(vx_it->second);
90  }
91  return m_output_vector;
92  }
93 
97  std::size_t size() const noexcept
98  {
99  return m_map.size();
100  }
101 
104  const ConfigPoint & cell_size() const noexcept
105  {
106  return m_config.get_voxel_size();
107  }
108 
111  typename Grid::const_iterator begin() const noexcept
112  {
113  return cbegin();
114  }
115 
118  typename Grid::iterator begin() noexcept
119  {
120  return m_map.begin();
121  }
122 
125  typename Grid::const_iterator cbegin() const noexcept
126  {
127  return m_map.cbegin();
128  }
129 
132  typename Grid::const_iterator end() const noexcept
133  {
134  return cend();
135  }
136 
139  typename Grid::iterator end() noexcept
140  {
141  return m_map.end();
142  }
143 
146  typename Grid::const_iterator cend() const noexcept
147  {
148  return m_map.cend();
149  }
150 
152  void clear() noexcept
153  {
154  m_map.clear();
155  }
156 
160  auto index(const Point & pt) const
161  {
162  return m_config.index(pt);
163  }
164 
168  template<typename ... Args>
169  auto emplace_voxel(Args && ... args)
170  {
171  return m_map.emplace(std::forward<Args>(args)...);
172  }
173 
176  void add_observation(const Point & pt)
177  {
178  m_map[index(pt)].add_observation(pt);
179  }
180 
183  void set_config(const Config & config)
184  {
185  m_config = config;
186  }
187 
190  const Config & config() const
191  {
192  return m_config;
193  }
194 
195 private:
196  mutable VoxelViewVector m_output_vector;
197  Config m_config;
198  Grid m_map;
199 };
200 
201 } // namespace ndt
202 } // namespace localization
203 
204 namespace common
205 {
206 namespace geometry
207 {
208 namespace point_adapter
209 {
213 template<>
214 inline NDT_PUBLIC auto x_(const Eigen::Vector3d & pt)
215 {
216  return static_cast<float32_t>(pt(0));
217 }
218 
219 template<>
220 inline NDT_PUBLIC auto y_(const Eigen::Vector3d & pt)
221 {
222  return static_cast<float32_t>(pt(1));
223 }
224 
225 template<>
226 inline NDT_PUBLIC auto z_(const Eigen::Vector3d & pt)
227 {
228  return static_cast<float32_t>(pt(2));
229 }
230 
231 template<>
232 inline NDT_PUBLIC auto & xr_(Eigen::Vector3d & pt)
233 {
234  return pt(0);
235 }
236 
237 template<>
238 inline NDT_PUBLIC auto & yr_(Eigen::Vector3d & pt)
239 {
240  return pt(1);
241 }
242 
243 template<>
244 inline NDT_PUBLIC auto & zr_(Eigen::Vector3d & pt)
245 {
246  return pt(2);
247 }
248 
249 } // namespace point_adapter
250 } // namespace geometry
251 } // namespace common
252 } // namespace autoware
253 #endif // NDT__NDT_GRID_HPP_
autoware::localization::ndt::NDTGrid< autoware::localization::ndt::DynamicNDTVoxel >::Point
Eigen::Vector3d Point
Definition: ndt_grid.hpp:44
autoware::localization::ndt::NDTGrid::emplace_voxel
auto emplace_voxel(Args &&... args)
Emplace a new voxel into the grid.
Definition: ndt_grid.hpp:169
autoware::localization::ndt::NDTGrid::set_config
void set_config(const Config &config)
Set the configuration.
Definition: ndt_grid.hpp:183
autoware::perception::filters::voxel_grid::Config::get_voxel_size
const PointXYZ & get_voxel_size() const
Gets the voxel size of the voxel grid.
Definition: perception/filters/voxel_grid/src/config.cpp:115
autoware::localization::ndt::NDTGrid::Config
autoware::perception::filters::voxel_grid::Config Config
Definition: ndt_grid.hpp:45
autoware::localization::ndt::NDTGrid::cbegin
Grid::const_iterator cbegin() const noexcept
Returns a const iterator to the first element of the map.
Definition: ndt_grid.hpp:125
autoware::common::geometry::point_adapter::x_
auto x_(const PointT &pt)
Gets the x value for a point.
Definition: common_2d.hpp:50
autoware::localization::ndt::NDTGrid::index
auto index(const Point &pt) const
Definition: ndt_grid.hpp:160
types.hpp
This file includes common type definition.
y
DifferentialState y
Definition: kinematic_bicycle.snippet.hpp:28
autoware::localization::ndt::NDTGrid::cell
const VoxelViewVector & cell(float32_t x, float32_t y, float32_t z) const
Definition: ndt_grid.hpp:73
autoware::localization::ndt::NDTGrid::NDTGrid
NDTGrid(const Config &voxel_grid_config)
Definition: ndt_grid.hpp:51
ndt_voxel_view.hpp
autoware::localization::ndt::NDTGrid
A voxel grid implementation for normal distribution transform.
Definition: ndt_grid.hpp:40
autoware::localization::ndt::NDTGrid::config
const Config & config() const
Get the underlying voxel grid configuration.
Definition: ndt_grid.hpp:190
autoware::perception::filters::voxel_grid::Config::get_min_point
const PointXYZ & get_min_point() const
Gets the minimum corner of the voxel grid.
Definition: perception/filters/voxel_grid/src/config.cpp:105
autoware::localization::ndt::NDTGrid::clear
void clear() noexcept
Clear all voxels in the map.
Definition: ndt_grid.hpp:152
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::localization::ndt::NDTGrid::operator=
NDTGrid & operator=(const NDTGrid &)=delete
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::localization::ndt::NDTGrid::begin
Grid::const_iterator begin() const noexcept
Returns an const iterator to the first element of the map.
Definition: ndt_grid.hpp:111
autoware::localization::ndt::NDTGrid::size
std::size_t size() const noexcept
Definition: ndt_grid.hpp:97
ndt_common.hpp
x
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
autoware::localization::ndt::NDTGrid< autoware::localization::ndt::DynamicNDTVoxel >::VoxelViewVector
std::vector< VoxelView< autoware::localization::ndt::DynamicNDTVoxel > > VoxelViewVector
Definition: ndt_grid.hpp:46
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::localization::ndt::NDTGrid< autoware::localization::ndt::DynamicNDTVoxel >::Grid
std::unordered_map< uint64_t, autoware::localization::ndt::DynamicNDTVoxel > Grid
Definition: ndt_grid.hpp:43
autoware::localization::ndt::NDTGrid< autoware::localization::ndt::DynamicNDTVoxel >::ConfigPoint
std::decay_t< decltype(std::declval< Config >().get_min_point())> ConfigPoint
Definition: ndt_grid.hpp:47
autoware::localization::ndt::NDTGrid::cell
const VoxelViewVector & cell(const Point &pt) const
Definition: ndt_grid.hpp:82
autoware::localization::ndt::NDTGrid::cell_size
const ConfigPoint & cell_size() const noexcept
Definition: ndt_grid.hpp:104
autoware::common::geometry::point_adapter::y_
auto y_(const PointT &pt)
Gets the y value for a point.
Definition: common_2d.hpp:66
autoware::localization::ndt::NDTGrid::cend
Grid::const_iterator cend() const noexcept
Returns a const iterator to one past the last element of the map.
Definition: ndt_grid.hpp:146
autoware::localization::ndt::NDTGrid::add_observation
void add_observation(const Point &pt)
Add a point to its corresponding voxel in the grid.
Definition: ndt_grid.hpp:176
autoware::common::geometry::point_adapter::z_
auto z_(const PointT &pt)
Gets the z value for a point.
Definition: common_2d.hpp:82
autoware::localization::ndt::NDTGrid::end
Grid::const_iterator end() const noexcept
Returns a const iterator to one past the last element of the map.
Definition: ndt_grid.hpp:132
autoware::localization::ndt::NDTGrid::end
Grid::iterator end() noexcept
Returns an iterator to one past the last element of the map.
Definition: ndt_grid.hpp:139
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::localization::ndt::NDTGrid::begin
Grid::iterator begin() noexcept
Returns an iterator to the first element of the map.
Definition: ndt_grid.hpp:118
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