UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
seen_empty_header.hpp
1
42#ifndef UFO_MAP_INTEGRATION_MISSES_HPP
43#define UFO_MAP_INTEGRATION_MISSES_HPP
44
45// UFO
46#include <ufo/map/code.hpp>
47#include <ufo/map/integration/grid.hpp>
48#include <ufo/map/types.hpp>
49#include <ufo/util/bit_set.hpp>
50
51// OMP
52#include <omp.h>
53
54// STL
55#include <cstdint>
56#include <cstdlib>
57#include <functional>
58#include <thread>
59#include <utility>
60#include <vector>
61
62namespace ufo::impl
63{
64
65struct Miss : public CodeOrIndex {
66 BitSet<8> sibling;
67 freedom_t freedom{};
68
69 constexpr Miss(Code code, BitSet<8> sibling, freedom_t freedom = 0)
70 : CodeOrIndex(code), sibling(sibling), freedom(freedom)
71 {
72 }
73};
74
75using Misses = std::vector<Miss>;
76
77template <std::size_t UnknownInflate = 0>
78Misses getMisses(CodeMap<Grid> const& free_grids, CodeMap<Grid> const& hit_grids,
79 depth_t const depth, std::size_t const num_threads)
80{
81 Misses misses;
82
83 std::vector<Code> codes;
84 codes.reserve(free_grids.size());
85 for (auto const& [code, _] : free_grids) {
86 codes.push_back(code);
87 }
88
89 auto isMaskSet = [](std::uint64_t const x, std::uint64_t const mask) {
90 return (x & mask) == mask;
91 };
92
93 code_t const inc = code_t(64) << 3 * depth;
94#pragma omp parallel num_threads(num_threads)
95 {
96 Misses thread_misses;
97
98#pragma omp for schedule(static)
99 for (auto const code : codes) {
100 auto thread_id = omp_get_thread_num();
101
102 auto const& free_grid = free_grids.find(code)->second;
103 // TODO: Add hit grid
104
105 code_t i = code.code();
106 for (auto e : free_grid) {
107 if (0 == e) {
108 i += inc;
109 continue;
110 }
111
112 std::array<std::uint64_t, 5> fg{};
113
114 std::array<std::uint64_t, 27> g{};
115 std::array<code_t, 27> c;
116
117 Key k_o = Code(i, depth + 2);
118
119 for (int z{-1}, s = k_o.step(), i{}; 2 != z; ++z) {
120 for (int y{-1}; 2 != y; ++y) {
121 for (int x{-1}; 2 != x; ++x, ++i) {
122 auto k = k_o;
123 k.x() += x * s;
124 k.y() += y * s;
125 k.z() += z * s;
126 Code c_k = k;
127 if (c_k.toDepth(code.depth()) == code) {
128 g[i] = free_grid[c_k.toDepth(depth)];
129 } else if (auto it = grids.find(c_k.toDepth(code.depth()));
130 std::end(grids) != it) {
131 g[i] = it->second[c_k.toDepth(depth)];
132 }
133 c[i] = c_k.code();
134 }
135 }
136 }
137
138 // Checks