Autoware.Auto
ndt_map.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_MAP_HPP_
18 #define NDT__NDT_MAP_HPP_
19 
20 #include <ndt/ndt_common.hpp>
21 #include <ndt/ndt_voxel.hpp>
22 #include <ndt/ndt_voxel_view.hpp>
23 #include <ndt/ndt_grid.hpp>
24 #include <sensor_msgs/point_cloud2_iterator.hpp>
26 #include <vector>
27 #include <limits>
28 #include <unordered_map>
29 #include <utility>
30 #include <string>
31 
33 
34 namespace autoware
35 {
36 namespace localization
37 {
38 namespace ndt
39 {
42 class NDT_PUBLIC DynamicNDTMap
43 {
44 public:
47  using Point = Eigen::Vector3d;
48  using TimePoint = std::chrono::system_clock::time_point;
49  using VoxelViewVector = std::vector<VoxelView<Voxel>>;
52 
55  static constexpr uint32_t kNumConfigPoints = 3U;
56 
57  explicit DynamicNDTMap(const Config & voxel_grid_config);
58 
61  void set(const sensor_msgs::msg::PointCloud2 & msg);
62 
66  void insert(const sensor_msgs::msg::PointCloud2 & msg);
67 
73  template<typename DeserializingMapT>
74  void serialize_as(sensor_msgs::msg::PointCloud2 & msg_out) const;
75 
80  const VoxelViewVector & cell(const Point & pt) const;
81 
88  const VoxelViewVector & cell(float32_t x, float32_t y, float32_t z) const;
89 
92  const std::string & frame_id() const noexcept;
93 
96  TimePoint stamp() const noexcept;
97 
100  bool valid() const noexcept;
101 
104  const ConfigPoint & cell_size() const noexcept;
105 
109  std::size_t size() const noexcept;
110 
113  typename VoxelGrid::const_iterator begin() const noexcept;
114 
117  typename VoxelGrid::const_iterator end() const noexcept;
118 
120  void clear() noexcept;
121 
122 private:
123  NDTGrid<DynamicNDTVoxel> m_grid;
124  TimePoint m_stamp{};
125  std::string m_frame_id{};
126 };
127 
131 class NDT_PUBLIC StaticNDTMap
132 {
133 public:
136  using TimePoint = std::chrono::system_clock::time_point;
137  using Point = Eigen::Vector3d;
138  using VoxelViewVector = std::vector<VoxelView<Voxel>>;
141 
150  void set(const sensor_msgs::msg::PointCloud2 & msg);
151 
156  const VoxelViewVector & cell(const Point & pt) const;
157 
164  const VoxelViewVector & cell(float32_t x, float32_t y, float32_t z) const;
165 
168  const std::string & frame_id() const noexcept;
169 
172  TimePoint stamp() const noexcept;
173 
176  bool valid() const noexcept;
177 
180  const ConfigPoint & cell_size() const;
181 
185  std::size_t size() const;
186 
189  typename VoxelGrid::const_iterator begin() const;
190 
193  typename VoxelGrid::const_iterator end() const;
194 
196  void clear();
197 
198 private:
201  void deserialize_from(const sensor_msgs::msg::PointCloud2 & msg);
202  std::experimental::optional<NDTGrid<StaticNDTVoxel>> m_grid{};
203  TimePoint m_stamp{};
204  std::string m_frame_id{};
205 };
206 } // namespace ndt
207 } // namespace localization
208 } // namespace autoware
209 #endif // NDT__NDT_MAP_HPP_
autoware::localization::ndt::DynamicNDTMap::VoxelViewVector
std::vector< VoxelView< Voxel > > VoxelViewVector
Definition: ndt_map.hpp:49
y
DifferentialState y
Definition: kinematic_bicycle.snippet.hpp:28
autoware::localization::ndt::DynamicNDTMap::ConfigPoint
NDTGrid< Voxel >::ConfigPoint ConfigPoint
Definition: ndt_map.hpp:51
ndt_voxel_view.hpp
autoware::localization::ndt::StaticNDTMap::Point
Eigen::Vector3d Point
Definition: ndt_map.hpp:137
autoware::localization::ndt::NDTGrid
A voxel grid implementation for normal distribution transform.
Definition: ndt_grid.hpp:40
autoware::localization::ndt::StaticNDTVoxel
Definition: ndt_voxel.hpp:101
autoware::localization::ndt::StaticNDTMap::VoxelGrid
NDTGrid< Voxel >::Grid VoxelGrid
Definition: ndt_map.hpp:139
autoware::localization::ndt::StaticNDTMap::ConfigPoint
NDTGrid< Voxel >::ConfigPoint ConfigPoint
Definition: ndt_map.hpp:140
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::localization::ndt::StaticNDTMap::TimePoint
std::chrono::system_clock::time_point TimePoint
Definition: ndt_map.hpp:136
autoware::localization::ndt::DynamicNDTMap::VoxelGrid
NDTGrid< Voxel >::Grid VoxelGrid
Definition: ndt_map.hpp:50
ndt_common.hpp
autoware::localization::ndt::DynamicNDTMap
Definition: ndt_map.hpp:42
x
DifferentialState x
Definition: kinematic_bicycle.snippet.hpp:28
ndt_grid.hpp
autoware::localization::ndt::StaticNDTMap::VoxelViewVector
std::vector< VoxelView< Voxel > > VoxelViewVector
Definition: ndt_map.hpp:138
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::localization::ndt::DynamicNDTMap::TimePoint
std::chrono::system_clock::time_point TimePoint
Definition: ndt_map.hpp:48
autoware::localization::ndt::StaticNDTMap
Definition: ndt_map.hpp:131
autoware::localization::ndt::NDTGrid::Grid
std::unordered_map< uint64_t, VoxelT > Grid
Definition: ndt_grid.hpp:43
autoware::localization::ndt::NDTGrid::ConfigPoint
std::decay_t< decltype(std::declval< Config >().get_min_point())> ConfigPoint
Definition: ndt_grid.hpp:47
time_utils.hpp
ndt_voxel.hpp
autoware::localization::ndt::DynamicNDTVoxel
Definition: ndt_voxel.hpp:46
autoware::perception::filters::filter_node_base::PointCloud2
sensor_msgs::msg::PointCloud2 PointCloud2
Definition: filter_node_base.cpp:32
get_open_port.end
end
Definition: scripts/get_open_port.py:23
autoware::localization::ndt::DynamicNDTMap::Point
Eigen::Vector3d Point
Definition: ndt_map.hpp:47