UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
header.hpp
1
42#ifndef UFO_MAP_MAP_HEADER_HPP
43#define UFO_MAP_MAP_HEADER_HPP
44
45// UFO
46#include <ufo/map/type.hpp>
47#include <ufo/numeric/vec.hpp>
48#include <ufo/utility/enum.hpp>
49#include <ufo/utility/io/buffer.hpp>
50#include <ufo/utility/string.hpp>
51
52// STL
53#if __cplusplus >= 202002L
54#include <bit>
55#endif
56#include <algorithm>
57#include <cctype>
58#include <cinttypes>
59#include <cstddef>
60#include <cstdint>
61#include <cstdio>
62#include <cstring>
63#include <filesystem>
64#include <fstream>
65#include <iostream>
66#include <locale>
67#include <sstream>
68#include <stdexcept>
69#include <string>
70#include <string_view>
71#include <vector>
72
73namespace ufo
74{
76 MapType type{};
77 std::uint64_t size{};
78
79 constexpr MapTypeInfo() = default;
80
81 MapTypeInfo(MapType type, std::uint64_t size = 0) : type(type), size(size) {}
82};
83
84struct MapHeader {
85 using length_t = double;
86 using depth_t = std::uint32_t;
87 using dim_t = std::uint32_t;
88 using version_t = std::uint32_t;
89
90 static constexpr std::string_view const MAP_HEADER = "# UFO Map";
91 static constexpr version_t const CURRENT_MAJOR = 1;
92 static constexpr version_t const CURRENT_MINOR = 0;
93 static constexpr version_t const CURRENT_PATCH = 0;
94 // static constexpr std::uint8_t IS_LITTLE_ENDIAN =
95 // std::endian::native == std::endian::little;
96
97 version_t major{};
98 version_t minor = std::numeric_limits<version_t>::max();
99 version_t patch = std::numeric_limits<version_t>::max();
100 // std::uint8_t is_little_endian;
101
102 std::vector<length_t> leaf_node_length;
103 depth_t num_depth_levels{};
104 std::uint64_t num_blocks{};
105 std::uint64_t num_nodes{};
106 std::vector<MapTypeInfo> map_info;
107
108 MapHeader() = default;
109 MapHeader(MapHeader const&) = default;
110
111 template <std::size_t Dim>
112 MapHeader(Vec<Dim, length_t> const& leaf_node_length, depth_t num_depth_levels,
113 MapType map_types)
114 : major(CURRENT_MAJOR)
115 , minor(CURRENT_MINOR)
116 , patch(CURRENT_PATCH)
117 , num_depth_levels(num_depth_levels)
118 {
119 this->leaf_node_length.reserve(Dim);
120 for (std::size_t i{}; Dim > i; ++i) {
121 this->leaf_node_length.push_back(leaf_node_length[i]);
122 }
123
124 // The tree
125 map_info.emplace_back(MapType::NONE);
126
127 // The map types
128 for (std::size_t i{};
129 std::numeric_limits<std::underlying_type_t<MapType>>::digits > i; ++i) {
130 MapType const type = static_cast<MapType>(std::underlying_type_t<MapType>(1) << i);
131 if (type != (map_types & type)) {
132 continue;
133 }
134 map_info.emplace_back(type);
135 }
136 }
137
138 template <class Map>
139 MapHeader(Map const& map)
140 : MapHeader(map.length(0), map.numDepthLevels(), map.mapTypes())
141 {
142 }
143
144 MapHeader(std::filesystem::path const& filename) { read(filename); }
145
146 MapHeader(std::istream& in) { read(in); }
147
148 MapHeader(ReadBuffer& in) { read(in); }
149
150 [[nodiscard]] static std::ifstream openRead(std::filesystem::path const& filename)
151 {
152 std::ifstream file;
153 file.exceptions(std::ifstream::badbit);
154 file.imbue(std::locale());
155 file.open(filename, std::ios::in | std::ios::binary);
156 return file;
157 }
158
159 [[nodiscard]] static std::ofstream openWrite(std::filesystem::path const& filename)
160 {
161 std::ofstream file;
162 file.exceptions(std::ifstream::badbit);
163 file.imbue(std::locale());
164 file.open(filename, std::ios::out | std::ios::binary);
165 return file;
166 }
167
168 [[nodiscard]] static bool isMap(std::filesystem::path const& filename)
169 {
170 auto file = openRead(filename);
171 return isMap(file);
172 }
173
174 [[nodiscard]] static bool isMap(std::istream& in)
175 {
176 std::string line;
177 return std::getline(in, line) && MAP_HEADER == line;
178 }
179
180 [[nodiscard]] static bool isMap(ReadBuffer& in)
181 {
182 std::string line;
183 return in.readLine(line) && MAP_HEADER != line;
184 }
185
186 void read(std::filesystem::path const& filename)
187 {
188 auto file = openRead(filename);
189 read(file);
190 }
191
192 std::istream& read(std::istream& in)
193 {
194 std::string line;
195
196 if (std::getline(in, line) && MAP_HEADER != line) {
197 throw std::runtime_error("Trying to read non-UFO Map");
198 }
199
200 // Reset
201 major = {};
202 minor = std::numeric_limits<version_t>::max();
203 patch = std::numeric_limits<version_t>::max();
204 leaf_node_length.clear();
205 num_depth_levels = {};
206 num_blocks = {};
207 num_nodes = {};
208 map_info.clear();
209
210 bool reading_map_info = false;
211 while (std::getline(in, line)) {
212 if (read(line, reading_map_info)) {
213 break;
214 }
215 }
216
217 if (version_t(0) == major || std::numeric_limits<version_t>::max() == minor ||
218 std::numeric_limits<version_t>::max() == patch) {
219 throw std::logic_error("Wrong or missing version '" + std::to_string(major) + "." +
220 std::to_string(minor) + "." + std::to_string(patch));
221 }
222
223 // TODO: Add more checks that all information is there
224
225 return in;
226 }
227
228 ReadBuffer& read(ReadBuffer& in)
229 {
230 std::string line;
231
232 if (in.readLine(line) && MAP_HEADER != line) {
233 throw std::runtime_error("Trying to read non-UFO Map");
234 }
235 // Reset
236 major = {};
237 minor = std::numeric_limits<version_t>::max();
238 patch = std::numeric_limits<version_t>::max();
239 leaf_node_length.clear();
240 num_depth_levels = {};
241 num_blocks = {};
242 num_nodes = {};
243 map_info.clear();
244
245 bool reading_map_info = false;
246 while (in.readLine(line)) {
247 if (read(line, reading_map_info)) {
248 break;
249 }
250 }
251
252 if (version_t(0) == major || std::numeric_limits<version_t>::max() == minor ||
253 std::numeric_limits<version_t>::max() == patch) {
254 throw std::logic_error("Wrong or missing version '" + std::to_string(major) + "." +
255 std::to_string(minor) + "." + std::to_string(patch));
256 }
257
258 // TODO: Add more checks that all information is there
259
260 return in;
261 }
262
263 void write(std::filesystem::path const& filename) const
264 {
265 auto file = openWrite(filename);
266 write(file);
267 }
268
269 std::ostream& write(std::ostream& out) const
270 {
271 out << MAP_HEADER << '\n';
272 out << "VERSION: " << major << '.' << minor << '.' << patch << '\n';
273 out << "LEAF_NODE_LENGTH: ";
274 // TODO: Make sure that enough precision is used for the leaf node length
275 if (2 == leaf_node_length.size()) {
276 out << Vec<2, length_t>(leaf_node_length[0], leaf_node_length[1]);
277 } else if (3 == leaf_node_length.size()) {
278 out << Vec<3, length_t>(leaf_node_length[0], leaf_node_length[1],
279 leaf_node_length[2]);
280 } else if (4 == leaf_node_length.size()) {
281 out << Vec<4, length_t>(leaf_node_length[0], leaf_node_length[1],
282 leaf_node_length[2], leaf_node_length[4]);
283 }
284 out << '\n';
285 out << "NUMBER_OF_DEPTH_LEVELS: " << num_depth_levels << '\n';
286 out << "NUM_BLOCKS: " << num_blocks << '\n';
287 out << "NUM_NODES: " << num_nodes << '\n';
288
289 out << "MAP_INFO:\n";
290
291 std::size_t longest_name = 4;
292 std::size_t longest_type = 4;
293 std::size_t longest_size = 12;
294 for (auto const& info : map_info) {
295 longest_name = std::max(longest_name, to_string(info.type).size());
296 std::stringstream ss;
297 ss << std::hex << std::showbase << to_underlying(info.type);
298 longest_type = std::max(longest_type, static_cast<std::size_t>(ss.tellp()));
299 ss.clear();
300 ss << std::hex << std::showbase << info.size;
301 longest_size = std::max(longest_size, static_cast<std::size_t>(ss.tellp()));
302 }
303
304 out << " # ";
305 out << std::setw(longest_name) << "Name";
306 out << ": " << std::setw(longest_type) << "Type";
307 out << ", " << std::setw(longest_size) << "Size (bytes)";
308 out << '\n';
309 for (auto const& info : map_info) {
310 out << " - ";
311 if (MapType::NONE == info.type) {
312 out << std::setw(longest_name) << "tree";
313 } else {
314 out << std::setw(longest_name) << to_string(info.type);
315 }
316 out << std::hex << std::showbase;
317 out << ": " << std::setw(longest_type) << to_underlying(info.type);
318 out << ", " << std::setw(longest_size) << info.size;
319 out << std::noshowbase << std::dec << '\n';
320 }
321
322 out << "DATA:\n";
323
324 return out;
325 }
326
327 WriteBuffer& write(WriteBuffer& out) const
328 {
329 std::ostringstream ss;
330 write(ss);
331 std::string s = ss.str();
332 out.write(s.data(), s.length());
333 return out;
334 }
335
336 private:
337 [[nodiscard]] bool read(std::string& line, bool& reading_map_info)
338 {
339 std::cout << line << '\n';
340 trim(line);
341 if (line.empty() || '#' == line[0]) {
342 return false;
343 }
344
345 auto it = std::find(line.begin(), line.end(), ':');
346 std::string param(line.begin(), it);
347 rtrim(param);
348
349 if (line.end() == it) {
350 throw std::logic_error("No value for parameter '" + param + "'");
351 }
352
353 std::string value(it + 1, line.end());
354 ltrim(value);
355
356 if (reading_map_info && '-' == param[0]) {
357 MapTypeInfo info;
358 std::uint64_t map_type = to_underlying(info.type);
359 if (2 !=
360 std::sscanf(value.c_str(), "%" SCNx64 ", %" SCNx64, &map_type, &info.size)) {
361 throw std::logic_error("Wrong map type info");
362 }
363 map_info.push_back(info);
364 return false;
365 }
366
367 reading_map_info = false;
368
369 if ("VERSION" == param) {
370 if (3 != std::sscanf(value.c_str(), "%" SCNu32 ".%" SCNu32 ".%" SCNu32, &major,
371 &minor, &patch)) {
372 throw std::logic_error("Wrong version format");
373 }
374 } else if ("LEAF_NODE_LENGTH" == param) {
375 leaf_node_length.resize(4);
376 auto dim =
377 std::sscanf(value.c_str(), "x: %lf y: %lf z: %lf w: %lf", &leaf_node_length[0],
378 &leaf_node_length[1], &leaf_node_length[2], &leaf_node_length[3]);
379 if (0 == dim) {
380 throw std::logic_error("Wrong leaf node length format");
381 }
382 leaf_node_length.resize(dim);
383 } else if ("NUMBER_OF_DEPTH_LEVELS" == param) {
384 num_depth_levels = std::stoul(value);
385 } else if ("NUM_BLOCKS" == param) {
386 num_blocks = std::stoull(value);
387 } else if ("NUM_NODES" == param) {
388 num_nodes = std::stoull(value);
389 } else if ("MAP_INFO" == param) {
390 reading_map_info = true;
391 } else if ("DATA" == param) {
392 return true;
393 }
394
395 return false;
396 }
397};
398
399inline std::ostream& operator<<(std::ostream& os, MapHeader const& header)
400{
401 return header.write(os);
402}
403
404inline std::istream& operator>>(std::istream& is, MapHeader& header)
405{
406 return header.read(is);
407}
408} // namespace ufo
409
410#endif // UFO_MAP_MAP_HEADER_HPP
Length length() const
Returns the length of the tree (/ root node), i.e. leaf_node_length * 2^depth().
Definition tree.hpp:342
constexpr depth_type numDepthLevels() const noexcept
Returns the number of depth levels of the tree, i.e. depth() + 1.
Definition tree.hpp:261
All vision-related classes and functions.
Definition cloud.hpp:49
void ltrim(std::string &s)
Removes leading whitespace from the string (in-place).
Definition string.cpp:106
void rtrim(std::string &s)
Removes trailing whitespace from the string (in-place).
Definition string.cpp:112
void trim(std::string &s)
Removes leading and trailing whitespace from the string (in-place).
Definition string.cpp:120
A fixed-size arithmetic vector of up to 4 dimensions.
Definition vec.hpp:76