UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
seen_empty_generator.cpp
1// STL
2#include <algorithm>
3#include <array>
4#include <cassert>
5#include <cmath>
6#include <fstream>
7#include <ios>
8#include <iostream>
9#include <sstream>
10#include <string>
11#include <unordered_map>
12#include <utility>
13#include <vector>
14
15std::vector<std::vector<std::vector<int>>> gridNums(std::size_t n)
16{
17 std::vector<std::vector<std::vector<int>>> grid_num(
18 4 * n, std::vector<std::vector<int>>(4 * n, std::vector<int>(4 * n)));
19
20 for (std::size_t x{}; grid_num.size() != x; ++x) {
21 for (std::size_t y{}; grid_num[x].size() != y; ++y) {
22 for (std::size_t z{}; grid_num[x][y].size() != z; ++z) {
23 grid_num[x][y][z] = (x / 4) + n * (y / 4) + n * n * (z / 4);
24 }
25 }
26 }
27 return grid_num;
28}
29
30std::string binaryToHex(std::string binary)
31{
32 static std::unordered_map<std::string, char> b2h = {
33 {"0000", '0'}, {"0001", '1'}, {"0010", '2'}, {"0011", '3'},
34 {"0100", '4'}, {"0101", '5'}, {"0110", '6'}, {"0111", '7'},
35 {"1000", '8'}, {"1001", '9'}, {"1010", 'A'}, {"1011", 'B'},
36 {"1100", 'C'}, {"1101", 'D'}, {"1110", 'E'}, {"1111", 'F'}};
37
38 while (binary.size() % 4) {
39 binary = '0' + binary;
40 }
41
42 std::string hex;
43 for (std::size_t i{}; i + 3 < binary.size(); i += 4) {
44 hex += b2h[binary.substr(i, 4)];
45 }
46
47 hex.erase(0, hex.find_first_not_of('0'));
48
49 return hex.empty() ? "0" : hex;
50}
51
52std::uint64_t binaryToUnsigned(std::string binary)
53{
54 std::uint64_t res{};
55 for (auto e : binary) {
56 res = res << 1 | ('1' == e);
57 }
58 return res;
59}
60
61std::vector<std::string> gen(std::vector<std::vector<std::vector<int>>> const& grid_num,
62 int const x, int const y, int const z, int const num)
63{
64 assert(0 <= num);
65
66 // TODO: Fix this so it works for other than 4
67 std::vector<std::string> c(27, std::string(64, '0'));
68
69 for (int ix{-num}; num >= ix; ++ix) {
70 for (int iy{-num}; num >= iy; ++iy) {
71 for (int iz{-num}; num >= iz; ++iz) {
72 auto mx = (x + ix) % 4;
73 auto my = (y + iy) % 4;
74 auto mz = (z + iz) % 4;
75 mx = mx > 1 ? mx + 6 : mx;
76 my = my > 1 ? my + 6 : my;
77 mz = mz > 1 ? mz + 6 : mz;
78 c[grid_num[x + ix][y + iy][z + iz]][63 - (mx + (2 * my) + (4 * mz))] = '1';
79 }
80 }
81 }
82
83 return c;
84}
85
86int main(int argc, char* argv[])
87{
88 // Settings
89 std::size_t max_unknown_inflate = 4;
90
91 std::ofstream output;
92
93 std::ostringstream res;
94
95 for (std::size_t ui{}; max_unknown_inflate >= ui; ++ui) {
96 res << "if constexpr (" << ui << " == UnknownInflate) {\n";
97
98 std::size_t a = std::ceil(ui / 4.0);
99 std::size_t n = 1 + 2 * a;
100
101 auto grid_num = gridNums(n);
102
103 res << "\tstd::array<std::uint64_t, " << (n * n * n) << "> g{};\n";
104 res << "\t\n";
105 res << "\tKey k_o = Code(i, depth + 2);\n";
106 res << "\t\n";
107 res << "\tfor (int z{-" << a << "}, s = k_o.step(), i{}; " << a << " >= z; ++z) {\n";
108 res << "\t\tfor (int y{-" << a << "}; " << a << " >= y; ++y) {\n";
109 res << "\t\t\tfor (int x{-" << a << "}; " << a << " >= x; ++x, ++i) {\n";
110 res << "\t\t\t\tauto k = k_o;\n";
111 res << "\t\t\t\tk.x() += x * s;\n";
112 res << "\t\t\t\tk.y() += y * s;\n";
113 res << "\t\t\t\tk.z() += z * s;\n";
114 res << "\t\t\t\tCode c_k = k;\n";
115 res << "\t\t\t\tif (Code::equalAtDepth(code, c_k, Grid::depth() + depth)) {\n";
116 res << "\t\t\t\t\tg[i] = free_grid[c_k.toDepth(depth)];\n";
117 res << "\t\t\t\t} else if (auto it = free_grids.find(c_k.toDepth(Grid::depth() + "
118 "depth)); "
119 "std::end(free_grids) != it) {\n";
120 res << "\t\t\t\t\tg[i] = it->second[c_k.toDepth(depth)];\n";
121 res << "\t\t\t\t}\n";
122 res << "\t\t\t}\n";
123 res << "\t\t}\n";
124 res << "\t}\n";
125 res << "\n";
126
127 std::size_t offset = 2 * (n - 1);
128 for (std::size_t z{offset}; offset + 4 != z; ++z) {
129 for (std::size_t y{offset}; offset + 4 != y; ++y) {
130 for (std::size_t x{offset}; offset + 4 != x; ++x) {
131 std::string node = z % 4 > 1 ? "1" : "0";
132 node += y % 4 > 1 ? "1" : "0";
133 node += x % 4 > 1 ? "1" : "0";
134 node += z % 2 ? "1" : "0";
135 node += y % 2 ? "1" : "0";
136 node += x % 2 ? "1" : "0";
137 auto const u = binaryToUnsigned(node);
138
139 auto const g = gen(grid_num, x, y, z, ui);
140 res << "mf[" << (u / 8) << "] |= std::uint_fast8_t((h & (1ull << " << u
141 << ")) && ";
142 for (std::size_t i{}; g.size() != i; ++i) {
143 auto const h = binaryToHex(g[i]);
144 if ("0" != h) {
145 res << "isMaskSet(g[" << i << "], 0x" << h << "ull) && ";
146 }
147 }
148 res.seekp(-4, std::ios_base::end);
149 res << ") << " << (u % 8) << ";\n";
150
151 // for (int d{}; 5 != d; ++d) {
152 // auto const g = gen(grid_num, x, y, z, ui);
153 // res << "if (";
154 // for (std::size_t i{}; g.size() != i; ++i) {
155 // auto const h = binaryToHex(g[i]);
156 // if ("0" != h) {
157 // res << "isMaskSet(g[" << i << "], 0x" << h << "ull) && ";
158 // }
159 // }
160 // res.seekp(-4, std::ios_base::end);
161 // res << ") {\n";
162 // }
163
164 // auto const u = binaryToUnsigned(node);
165 // res << "\tfg[4] |= std::uint64_t(1) << " << u << ";\n";
166 // for (int d = 3; -1 != d; --d) {
167 // res << "} else {\n";
168 // res << "\tfg[" << d << "] |= std::uint64_t(1) << " << u << ";\n}\n";
169 // }
170 // res << "}\n\n";
171 }
172 }
173 }
174
175 res << "} else ";
176 }
177 res.seekp(-5, std::ios_base::end);
178 res << " ";
179
180 output.open("/home/dduberg/ufomap2/include/ufo/map/integration/misses/1.hpp",
181 std::ios::out | std::ios::binary);
182 output << res.str();
183 output.close();
184
185 res = {};
186 res << "\tswitch (unknown_inflate) {\n";
187 for (int ui{}; max_unknown_inflate != ui; ++ui) {
188 res << "\t\tcase " << ui << ": return getMisses<" << ui
189 << ">(free_grids, hit_grids, depth, num_threads);\n";
190 }
191 res << "\t\tdefault: return getMisses<" << max_unknown_inflate
192 << ">(free_grids, hit_grids, depth, num_threads);\n";
193 res << "\t}\n";
194
195 output.open("/home/dduberg/ufomap2/include/ufo/map/integration/misses/2.hpp",
196 std::ios::out | std::ios::binary);
197 output << res.str();
198 output.close();
199
200 return 0;
201}
constexpr T a(Lab< T, Flags > color) noexcept
Returns the un-weighted green–red axis value.
Definition lab.hpp:310