UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
semantic_set_predicate.hpp
1
42#ifndef UFO_MAP_SEMANTIC_SET_PREDICATE_HPP
43#define UFO_MAP_SEMANTIC_SET_PREDICATE_HPP
44
45// UFO
46#include <ufo/map/node.hpp>
47#include <ufo/map/predicate/predicate.hpp>
48// #include <ufo/map/semantic/semantic.hpp>
49#include <ufo/map/semantic_set/semantic_set_map.hpp>
50#include <ufo/map/types.hpp>
51
52namespace ufo::pred
53{
54template <bool Negated = false>
55struct SemanticEmpty {
56};
57
58//
59// SemanticSetMap
60//
61
62struct SemanticSetMap {
63};
64
65template <>
66struct ValueCheck<SemanticSetMap> {
67 using Pred = SemanticSetMap;
68
69 template <class Map, class Node>
70 static constexpr bool apply(Pred, Map const&, Node)
71 {
72 return is_semantic_set_map_v<Map>;
73 }
74};
75
76template <class PredPost>
77struct ValueCheck<Then<SemanticSetMap, PredPost>> {
78 using Pred = Then<SemanticSetMap, PredPost>;
79
80 template <class Map, class Node>
81 static constexpr bool apply(Pred const& p, Map const& m, Node const& n)
82 {
83 if constexpr (ValueCheck<SemanticSetMap>::apply(p.pre, m, n)) {
84 return ValueCheck<PredPost>::apply(p.post, m, n);
85 } else {
86 return true;
87 }
88 }
89};
90
91template <>
92struct InnerCheck<SemanticSetMap> {
93 using Pred = SemanticSetMap;
94
95 template <class Map, class Node>
96 static constexpr bool apply(Pred, Map const&, Node)
97 {
98 return is_semantic_set_map_v<Map>;
99 }
100};
101
102template <class PredPost>
103struct InnerCheck<Then<SemanticSetMap, PredPost>> {
104 using Pred = Then<SemanticSetMap, PredPost>;
105
106 template <class Map, class Node>
107 static constexpr bool apply(Pred const& p, Map const& m, Node const& n)
108 {
109 if constexpr (InnerCheck<SemanticSetMap>::apply(p.pre, m, n)) {
110 return InnerCheck<PredPost>::apply(p.post, m, n);
111 } else {
112 return true;
113 }
114 }
115};
116
117//
118// Semantic
119//
120
121template <bool Negated = false>
122struct SemanticSetLabel {
123 constexpr SemanticSetLabel(label_t label) : label(label) {}
124
125 label_t label;
126};
127
128template <bool Negated>
129constexpr SemanticSetLabel<!Negated> operator!(SemanticSetLabel<Negated> p)
130{
131 return SemanticSetLabel<!Negated>(p.label);
132}
133
134template <bool Negated>
135struct ValueCheck<SemanticSetLabel<Negated>> {
136 using Pred = SemanticSetLabel<Negated>;
137
138 template <class Map, class Node>
139 static inline bool apply(Pred p, Map const& m, Node n)
140 {
141 if constexpr (Negated) {
142 // return p.label != (m.semantic(n.index()).label & p.label);
143 // return !m.semantics(n.index()).contains(p.label);
144 return !m.containsSemantics(n.index(), p.label);
145 } else {
146 // return p.label == (m.semantic(n.index()).label & p.label);
147 // return m.semantics(n.index()).contains(p.label);
148 return m.containsSemantics(n.index(), p.label);
149 }
150 }
151};
152
153template <bool Negated>
154struct InnerCheck<SemanticSetLabel<Negated>> {
155 using Pred = SemanticSetLabel<Negated>;
156
157 template <class Map, class Node>
158 static inline bool apply(Pred p, Map const& m, Node n)
159 {
160 if constexpr (Negated) {
161 return true; // FIXME: Can this be better?
162 } else {
163 switch (m.semanticSetPropagationCriteria()) {
164 case ufo::impl::SemanticSetPropagationCriteria::MIN:
165 case ufo::impl::SemanticSetPropagationCriteria::MAX:
166 case ufo::impl::SemanticSetPropagationCriteria::NONE:
167 return m.semantics(n.index()).contains(p.label);
168
169 case ufo::impl::SemanticSetPropagationCriteria::S_MIN:
170 case ufo::impl::SemanticSetPropagationCriteria::S_MAX:
171 return p.label == (m.semanticSetSummary(n.index()).label & p.label);
172 }
173 return true;
174 }
175 }
176};
177
178//
179// Any
180//
181
182// Tag
183
184template <bool Negated = false>
185struct AnySemanticTag {
186 constexpr AnySemanticTag(std::initializer_list<std::string> tag) : tags(tag) {}
187
188 std::vector<std::string> tags;
189};
190
191template <bool Negated>
192constexpr AnySemanticTag<!Negated> operator!(AnySemanticTag<Negated> p)
193{
194 return AnySemanticTag<!Negated>(p.tags);
195}
196
197template <bool Negated>
198struct ValueCheck<AnySemanticTag<Negated>> {
199 using Pred = AnySemanticTag<Negated>;
200
201 template <class Map, class Node>
202 static inline bool apply(Pred p, Map const& m, Node n)
203 {
204 if constexpr (Negated) {
205 return !m.anySemantics(n.index(), p.tags.begin(), p.tags.end());
206 } else {
207 return m.anySemantics(n.index(), p.tags.begin(), p.tags.end());
208 }
209 }
210};
211
212template <bool Negated>
213struct InnerCheck<AnySemanticTag<Negated>> {
214 using Pred = AnySemanticTag<Negated>;
215
216 template <class Map, class Node>
217 static inline bool apply(Pred p, Map const& m, Node n)
218 {
219 if constexpr (Negated) {
220 return true; // FIXME: Can this be better?
221 } else {
222 switch (m.semanticSetPropagationCriteria()) {
223 case ufo::impl::SemanticSetPropagationCriteria::MIN:
224 case ufo::impl::SemanticSetPropagationCriteria::MAX:
225 case ufo::impl::SemanticSetPropagationCriteria::NONE:
226 return m.anySemantics(n.index(), p.tags.begin(), p.tags.end());
227
228 case ufo::impl::SemanticSetPropagationCriteria::S_MIN:
229 case ufo::impl::SemanticSetPropagationCriteria::S_MAX:
230 auto summary_label = m.semanticSetSummary(n.index()).label;
231 for (auto tag : p.tags) {
232 auto labelset = m.labels(tag); // rangeset
233 for (auto labels : labelset) { // range
234 for (auto l = labels.lower(); l <= labels.upper(); ++l) {
235 if (l == (summary_label & l)) {
236 return true;
237 }
238 }
239 }
240 }
241 return false;
242 }
243 return true;
244 }
245 }
246};
247
248// Label
249
250template <bool Negated = false>
251struct AnySemanticLabel {
252 constexpr AnySemanticLabel(std::initializer_list<label_t> label) : labels(label) {}
253
254 std::vector<label_t> labels;
255};
256
257template <bool Negated>
258constexpr AnySemanticLabel<!Negated> operator!(AnySemanticLabel<Negated> p)
259{
260 return AnySemanticLabel<!Negated>(p.labels);
261}
262
263template <bool Negated>
264struct ValueCheck<AnySemanticLabel<Negated>> {
265 using Pred = AnySemanticLabel<Negated>;
266
267 template <class Map, class Node>
268 static inline bool apply(Pred p, Map const& m, Node n)
269 {
270 if constexpr (Negated) {
271 return !m.anySemantics(n.index(),
272 SemanticRangeSet(p.labels.begin(), p.labels.end()));
273 } else {
274 return m.anySemantics(n.index(),
275 SemanticRangeSet(p.labels.begin(), p.labels.end()));
276 }
277 }
278};
279
280template <bool Negated>
281struct InnerCheck<AnySemanticLabel<Negated>> {
282 using Pred = AnySemanticLabel<Negated>;
283
284 template <class Map, class Node>
285 static inline bool apply(Pred p, Map const& m, Node n)
286 {
287 if constexpr (Negated) {
288 return true; // FIXME: Can this be better?
289 } else {
290 switch (m.semanticSetPropagationCriteria()) {
291 case ufo::impl::SemanticSetPropagationCriteria::MIN:
292 case ufo::impl::SemanticSetPropagationCriteria::MAX:
293 case ufo::impl::SemanticSetPropagationCriteria::NONE:
294 return m.anySemantics(n.index(),
295 SemanticRangeSet(p.labels.begin(), p.labels.end()));
296
297 case ufo::impl::SemanticSetPropagationCriteria::S_MIN:
298 case ufo::impl::SemanticSetPropagationCriteria::S_MAX:
299 auto summary_label = m.semanticSetSummary(n.index()).label;
300 for (auto l : p.labels) { // range
301 if (l == (summary_label & l)) {
302 return true;
303 }
304 }
305 return false;
306 }
307 return true;
308 }
309 }
310};
311
312//
313// All
314//
315
316// Tag
317
318template <bool Negated = false>
319struct AllSemanticTag {
320 constexpr AllSemanticTag(std::initializer_list<std::string> tag) : tags(tag) {}
321
322 std::vector<std::string> tags;
323};
324
325template <bool Negated>
326constexpr AllSemanticTag<!Negated> operator!(AllSemanticTag<Negated> p)
327{
328 return AllSemanticTag<!Negated>(p.tags);
329}
330
331template <bool Negated>
332struct ValueCheck<AllSemanticTag<Negated>> {
333 using Pred = AllSemanticTag<Negated>;
334
335 template <class Map, class Node>
336 static inline bool apply(Pred p, Map const& m, Node n)
337 {
338 if constexpr (Negated) {
339 return !m.allSemantics(n.index(), p.tags.begin(), p.tags.end());
340 } else {
341 return m.allSemantics(n.index(), p.tags.begin(), p.tags.end());
342 }
343 }
344};
345
346template <bool Negated>
347struct InnerCheck<AllSemanticTag<Negated>> {
348 using Pred = AllSemanticTag<Negated>;
349
350 template <class Map, class Node>
351 static inline bool apply(Pred p, Map const& m, Node n)
352 {
353 if constexpr (Negated) {
354 return true; // FIXME: Can this be better?
355 } else {
356 switch (m.semanticSetPropagationCriteria()) {
357 case ufo::impl::SemanticSetPropagationCriteria::MIN:
358 case ufo::impl::SemanticSetPropagationCriteria::MAX:
359 case ufo::impl::SemanticSetPropagationCriteria::NONE:
360 return m.allSemantics(n.index(), p.tags.begin(), p.tags.end());
361
362 case ufo::impl::SemanticSetPropagationCriteria::S_MIN:
363 case ufo::impl::SemanticSetPropagationCriteria::S_MAX:
364 auto summary_label = m.semanticSetSummary(n.index()).label;
365 for (auto tag : p.tags) {
366 auto labelset = m.labels(tag); // rangeset
367 for (auto labels : labelset) { // range
368 for (auto l = labels.lower(); l <= labels.upper(); ++l) {
369 if (l != (summary_label & l)) {
370 return false;
371 }
372 }
373 }
374 }
375 return true;
376 }
377 return true;
378 }
379 }
380};
381
382// Label
383
384template <bool Negated = false>
385struct AllSemanticLabel {
386 constexpr AllSemanticLabel(std::initializer_list<label_t> label) : labels(label) {}
387
388 std::vector<label_t> labels;
389};
390
391template <bool Negated>
392constexpr AllSemanticLabel<!Negated> operator!(AllSemanticLabel<Negated> p)
393{
394 return AllSemanticLabel<!Negated>(p.labels);
395}
396
397template <bool Negated>
398struct ValueCheck<AllSemanticLabel<Negated>> {
399 using Pred = AllSemanticLabel<Negated>;
400
401 template <class Map, class Node>
402 static inline bool apply(Pred p, Map const& m, Node n)
403 {
404 if constexpr (Negated) {
405 return !m.allSemantics(n.index(),
406 SemanticRangeSet(p.labels.begin(), p.labels.end()));
407 } else {
408 return m.allSemantics(n.index(),
409 SemanticRangeSet(p.labels.begin(), p.labels.end()));
410 }
411 }
412};
413
414template <bool Negated>
415struct InnerCheck<AllSemanticLabel<Negated>> {
416 using Pred = AllSemanticLabel<Negated>;
417
418 template <class Map, class Node>
419 static inline bool apply(Pred p, Map const& m, Node n)
420 {
421 if constexpr (Negated) {
422 return true; // FIXME: Can this be better?
423 } else {
424 switch (m.semanticSetPropagationCriteria()) {
425 case ufo::impl::SemanticSetPropagationCriteria::MIN:
426 case ufo::impl::SemanticSetPropagationCriteria::MAX:
427 case ufo::impl::SemanticSetPropagationCriteria::NONE:
428 return m.allSemantics(n.index(),
429 SemanticRangeSet(p.labels.begin(), p.labels.end()));
430
431 case ufo::impl::SemanticSetPropagationCriteria::S_MIN:
432 case ufo::impl::SemanticSetPropagationCriteria::S_MAX:
433 auto summary_label = m.semanticSetSummary(n.index()).label;
434 for (auto l : p.labels) { // range
435 if (l != (summary_label & l)) {
436 return false;
437 }
438 }
439 return true;
440 }
441 return true;
442 }
443 }
444};
445
446//
447// None
448//
449
450// Tag
451
452template <bool Negated = false>
453struct NoneSemanticTag {
454 constexpr NoneSemanticTag(std::initializer_list<std::string> tag) : tags(tag) {}
455
456 std::vector<std::string> tags;
457};
458
459template <bool Negated>
460constexpr NoneSemanticTag<!Negated> operator!(NoneSemanticTag<Negated> p)
461{
462 return NoneSemanticTag<!Negated>(p.tags);
463}
464
465template <bool Negated>
466struct ValueCheck<NoneSemanticTag<Negated>> {
467 using Pred = NoneSemanticTag<Negated>;
468
469 template <class Map, class Node>
470 static inline bool apply(Pred p, Map const& m, Node n)
471 {
472 if constexpr (Negated) {
473 return !m.noneSemantics(n.index(), p.tags.begin(), p.tags.end());
474 } else {
475 return m.noneSemantics(n.index(), p.tags.begin(), p.tags.end());
476 }
477 }
478};
479
480template <bool Negated>
481struct InnerCheck<NoneSemanticTag<Negated>> {
482 using Pred = NoneSemanticTag<Negated>;
483
484 template <class Map, class Node>
485 static inline bool apply(Pred p, Map const& m, Node n)
486 {
487 if constexpr (Negated) {
488 return true; // FIXME: Can this be better?
489 } else {
490 switch (m.semanticSetPropagationCriteria()) {
491 case ufo::impl::SemanticSetPropagationCriteria::MIN:
492 case ufo::impl::SemanticSetPropagationCriteria::MAX:
493 case ufo::impl::SemanticSetPropagationCriteria::NONE:
494 return m.noneSemantics(n.index(), p.tags.begin(), p.tags.end());
495
496 case ufo::impl::SemanticSetPropagationCriteria::S_MIN:
497 case ufo::impl::SemanticSetPropagationCriteria::S_MAX: return true;
498 }
499 return true;
500 }
501 }
502};
503
504// Label
505
506template <bool Negated = false>
507struct NoneSemanticLabel {
508 constexpr NoneSemanticLabel(std::initializer_list<label_t> label) : labels(label) {}
509
510 std::vector<label_t> labels;
511};
512
513template <bool Negated>
514constexpr NoneSemanticLabel<!Negated> operator!(NoneSemanticLabel<Negated> p)
515{
516 return NoneSemanticLabel<!Negated>(p.labels);
517}
518
519template <bool Negated>
520struct ValueCheck<NoneSemanticLabel<Negated>> {
521 using Pred = NoneSemanticLabel<Negated>;
522
523 template <class Map, class Node>
524 static inline bool apply(Pred p, Map const& m, Node n)
525 {
526 if constexpr (Negated) {
527 return !m.noneSemantics(n.index(),
528 SemanticRangeSet(p.labels.begin(), p.labels.end()));
529 } else {
530 return m.noneSemantics(n.index(),
531 SemanticRangeSet(p.labels.begin(), p.labels.end()));
532 }
533 }
534};
535
536template <bool Negated>
537struct InnerCheck<NoneSemanticLabel<Negated>> {
538 using Pred = NoneSemanticLabel<Negated>;
539
540 template <class Map, class Node>
541 static inline bool apply(Pred p, Map const& m, Node n)
542 {
543 if constexpr (Negated) {
544 return true; // FIXME: Can this be better?
545 } else {
546 switch (m.semanticSetPropagationCriteria()) {
547 case ufo::impl::SemanticSetPropagationCriteria::MIN:
548 case ufo::impl::SemanticSetPropagationCriteria::MAX:
549 case ufo::impl::SemanticSetPropagationCriteria::NONE:
550 return m.noneSemantics(n.index(),
551 SemanticRangeSet(p.labels.begin(), p.labels.end()));
552
553 case ufo::impl::SemanticSetPropagationCriteria::S_MIN:
554 case ufo::impl::SemanticSetPropagationCriteria::S_MAX: return true;
555 }
556 return true;
557 }
558 }
559};
560
561} // namespace ufo::pred
562
563#endif // UFO_MAP_SEMANTIC_SET_PREDICATE_HPP
ufo::contains
constexpr bool contains(A const &a, B const &b)
Checks if a shape contains another shape.
Definition contains.hpp:63