UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
labels_map.hpp
1
42#ifndef UFO_MAP_LABEL_MAP_HPP
43#define UFO_MAP_LABEL_MAP_HPP
44
45// UFO
46#include <ufo/map/code.hpp>
47#include <ufo/map/index.hpp>
48#include <ufo/map/key.hpp>
49#include <ufo/map/node.hpp>
50#include <ufo/map/point.hpp>
51#include <ufo/map/types.hpp>
52#include <ufo/util/bit_set.hpp>
53#include <ufo/util/buffer.hpp>
54
55// STL
56#include <algorithm>
57#include <array>
58#include <deque>
59#include <functional>
60#include <iostream>
61#include <limits>
62#include <utility>
63
64namespace ufo
65{
66template <class Derived, std::size_t N>
68{
69 public:
70 //
71 // Get label
72 //
73
74 [[nodiscard]] constexpr label_t label(Node node) const
75 {
76 auto [index, offset] = derived().indexAndOffset(node);
77 return label_[index][offset];
78 }
79
80 [[nodiscard]] label_t label(Code code) const
81 {
82 auto [index, offset] = derived().indexAndOffset(code);
83 return label_[index][offset];
84 }
85
86 [[nodiscard]] label_t label(Key key) const { return label(derived().toCode(key)); }
87
88 [[nodiscard]] label_t label(Point coord, depth_t depth = 0) const
89 {
90 return label(derived().toCode(coord, depth));
91 }
92
93 //
94 // TODO: Add label
95 //
96
97 //
98 // TODO: Remove label
99 //
100
101 protected:
102 //
103 // Constructors
104 //
105
106 LabelMap() = default;
107
108 LabelMap(LabelMap const& other) = default;
109
110 LabelMap(LabelMap&& other) = default;
111
112 template <class Derived2>
113 LabelMap(LabelMap<Derived2> const& other) : label_(other.label_)
114 {
115 }
116
117 //
118 // Assignment operator
119 //
120
121 LabelMap& operator=(LabelMap const& rhs) = default;
122
123 LabelMap& operator=(LabelMap&& rhs) = default;
124
125 template <class Derived2>
126 LabelMap& operator=(LabelMap<Derived2> const& rhs)
127 {
128 label_ = rhs.label_;
129 return *this;
130 }
131
132 //
133 // Swap
134 //
135
136 void swap(LabelMap& other) noexcept { std::swap(prop_criteria_, other.prop_criteria_); }
137
138 //
139 // Derived
140 //
141
142 [[nodiscard]] constexpr Derived& derived() { return *static_cast<Derived*>(this); }
143
144 [[nodiscard]] constexpr Derived const& derived() const
145 {
146 return *static_cast<Derived const*>(this);
147 }
148
149 //
150 // Initialize root
151 //
152
153 void initRoot() { label_[derived().rootIndex()][derived().rootOffset()].clear(); }
154
155 //
156 // Fill
157 //
158
159 void fill(index_t index, index_t parent_index, index_t parent_offset)
160 {
161 intensity_[index].fill(intensity_[parent_index][parent_offset]);
162 }
163
164 //
165 // Clear
166 //
167
168 void clearImpl() {}
169
170 void clearImpl(index_t index) {}
171
172 //
173 // Update block
174 //
175
176 void updateBlock(pos_t block)
177 {
178 // TODO implement
179 }
180
181 void updateNode(index_t index, index_t offset, index_t children_index)
182 {
183 switch (intensityPropagationCriteria()) {
184 case PropagationCriteria::MIN:
185 intensity_[index][offset] = min(intensity_[children_index]);
186 return;
187 case PropagationCriteria::MAX:
188 intensity_[index][offset] = max(intensity_[children_index]);
189 return;
190 case PropagationCriteria::MEAN:
191 intensity_[index][offset] = mean(intensity_[children_index]);
192 return;
193 }
194 }
195
196 //
197 // Is collapsible
198 //
199
200 [[nodiscard]] constexpr bool isCollapsible(index_t index) const
201 {
202 return std::all_of(std::begin(intensity_[index]) + 1, std::end(intensity_[index]),
203 [t = intensity_[index].front()](auto e) { return e == t; });
204 }
205
206 void preparePrune(Index node) {}
207
208 //
209 // Input/output (read/write)
210 //
211
212 [[nodiscard]] static constexpr MapType mapType() noexcept { return MapType::INTENSITY; }
213
214 [[nodiscard]] static constexpr bool canReadData(MapType mt) noexcept
215 {
216 return mapType() == mt;
217 }
218
219 template <class InputIt>
220 std::size_t serializedSize(InputIt first, InputIt last) const
221 {
222 return std::distance(first, last) * N * sizeof(intensity_t);
223 }
224
225 template <class OutputIt>
226 void readNodes(ReadBuffer& in, OutputIt first, OutputIt last)
227 {
228 for (; first != last; ++first) {
229 if (first->offsets.all()) {
230 in.read(intensity_[first->index].data(), N * sizeof(intensity_t));
231 } else {
232 std::array<intensity_t, N> intensity;
233 in.read(intensity.data(), N * sizeof(intensity_t));
234 for (index_t i = 0; N != i; ++i) {
235 if (first->offsets[i]) {
236 intensity_[first->index][i] = intensity[i];
237 }
238 }
239 }
240 }
241 }
242
243 template <class BlockRange>
244 void writeBlocks(WriteBuffer& out, BlockRange const& blocks) const
245 {
246 for (auto block : blocks) {
247 // TODO: Implement
248 }
249 }
250
251 protected:
252 // Data
253 std::deque<LabelSet> label_;
254
255 template <class Derived2, std::size_t N2>
256 friend class LabelMap;
257};
258} // namespace ufo
259
260namespace ufo
261{
262//
263// Type traits
264//
265
266template <class Map>
268 : std::conditional_t<is_map_type_v<Map, MapType::LABEL>, std::true_type,
269 std::false_type> {
270};
271template <class Map>
272constexpr inline bool is_labels_map_v = is_labels_map<Map>::value;
273} // namespace ufo
274#endif // UFO_MAP_INTENSITY_MAP_HPP
All vision-related classes and functions.
Definition cloud.hpp:49
constexpr Vec< Geometry::dimension(), typename Geometry::value_type > max(Geometry const &g)
Returns the maximum coordinate of the minimum spanning axis-aligned bounding box of a geometry.
Definition fun.hpp:72
constexpr Vec< Geometry::dimension(), typename Geometry::value_type > min(Geometry const &g)
Returns the minimum coordinate of the minimum spanning axis-aligned bounding box of a geometry.
Definition fun.hpp:58