UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
semantic_set.hpp
1
42#ifndef UFO_MAP_SEMANTIC_SET_HPP
43#define UFO_MAP_SEMANTIC_SET_HPP
44
45// UFO
46#include <ufo/container/range.hpp>
47#include <ufo/container/range_map.hpp>
48#include <ufo/util/iterator_wrapper.hpp>
49#include <ufo/util/type_traits.hpp>
50// #include <ufo/map/semantic/semantics_reference.h>
51#include <ufo/map/semantic/semantic.hpp>
52#include <ufo/map/semantic_set/semantic_util.hpp>
53#include <ufo/util/bit_set.hpp>
54
55// STL
56#include <algorithm>
57#include <cstddef> // For std::ptrdiff_t
58#include <functional>
59#include <initializer_list>
60#include <istream>
61#include <iterator> // For std::random_access_iterator_tag / std::contiguous_iterator_tag
62#include <memory>
63#include <ostream>
64#include <stdexcept>
65#include <utility>
66#include <vector>
67
68namespace ufo
69{
70template <std::size_t N = 1>
72{
73 public:
74 // Tags
75 using value_type = Semantic;
76 using size_type = label_t;
77 using difference_type = std::ptrdiff_t;
78 using reference = Semantic&; // TODO: Make label const
79 using const_reference = Semantic const&;
80 using pointer = Semantic*;
81 using const_pointer = Semantic const*;
82 using iterator = Semantic*; // TODO: Make label const
83 using const_iterator = Semantic const*;
84 using reverse_iterator = std::reverse_iterator<iterator>;
85 using const_reverse_iterator = std::reverse_iterator<const_iterator>;
86
87 // -1 half rounded up (i.e., the number of elements needed to store the sizes of the N
88 // semantic containers)
89 static constexpr std::size_t N_H = 1 + (N - 1) / 2;
90
91 // Data
92 // TODO: Most significate size bits indicates whether the labels have change and the
93 // second most significate if the values have changed for the particular index.
94 std::unique_ptr<Semantic[]> semantics;
95
96 SemanticSet() {}
97
98 // copy constructor
99 SemanticSet(SemanticSet const& other)
100 {
101 for (offset_t i{}; i != N; ++i) {
102 set(i, other.begin(i), other.end(i));
103 }
104 }
105
106 // move constructor
107 SemanticSet(SemanticSet&& other) noexcept : semantics(std::move(other.semantics)) {}
108
109 // move assignment operator
110 SemanticSet& operator=(SemanticSet&& other) noexcept
111 {
112 semantics = std::move(other.semantics);
113 return *this;
114 }
115
116 // copy assignment operator
117 SemanticSet& operator=(SemanticSet const& other)
118 {
119 for (offset_t i{}; i != N; ++i) {
120 set(i, other.begin(i), other.end(i));
121 }
122 return *this;
123 }
124
125 ~SemanticSet() {}
126
127 //
128 // Size
129 //
130
131 [[nodiscard]] static constexpr std::size_t semanticSize() { return N; }
132
133 [[nodiscard]] size_type semanticAllocSize() const
134 {
135 return empty() ? 0 : semanticSize() + N_H;
136 }
137
138 //
139 // Fill
140 //
141
142 // fill all our nodes with data from parent node with specified index
143 void fill(SemanticSet const& parent, offset_t const index)
144 {
145 semantic::resizeLazy<N>(semantics, parent.size(index));
146 auto first = parent.begin(index);
147 auto last = parent.end(index);
148 for (offset_t i = 0; N != i; ++i) {
149 std::copy(first, last, begin(i));
150 }
151 }
152
153 //
154 // Is collapsible
155 //
156
157 // check if all nodes have exactly the same contents
158 [[nodiscard]] bool isPrunable() const
159 {
160 auto first = cbegin(0);
161 auto last = cend(0);
162 for (offset_t i = 1; N != i; ++i) {
163 if (!std::equal(first, last, cbegin(i), cend(i))) {
164 return false;
165 }
166 }
167 return true;
168 }
169
170 // // check if all children have exactly the same elements as parent at specified index
171 // [[nodiscard]] bool isCollapsible(SemanticSet const &parent, offset_t const index)
172 // const
173 // {
174 // auto first = parent.begin(index);
175 // auto last = parent.end(index);
176 // for (offset_t i = 0; N != i; ++i) {
177 // if (!std::equal(first, last, cbegin(i), cend(i))) {
178 // return false;
179 // }
180 // }
181 // return true;
182 // }
183
184 //
185 // Iterators
186 //
187
188 iterator begin() noexcept { return semantic::begin<N>(semantics); }
189
190 const_iterator begin() const noexcept { return semantic::begin<N>(semantics); }
191
192 const_iterator cbegin() const noexcept { return begin(); }
193
194 iterator end() noexcept { return semantic::end<N>(semantics); }
195
196 const_iterator end() const noexcept { return semantic::end<N>(semantics); }
197
198 const_iterator cend() const noexcept { return end(); }
199
200 //
201 // Reverse iterators
202 //
203
204 reverse_iterator rbegin() noexcept { return std::make_reverse_iterator(end()); }
205
206 const_reverse_iterator rbegin() const noexcept
207 {
208 return std::make_reverse_iterator(end());
209 }
210
211 const_reverse_iterator crbegin() const noexcept { return rbegin(); }
212
213 reverse_iterator rend() noexcept { return std::make_reverse_iterator(begin()); }
214
215 const_reverse_iterator rend() const noexcept
216 {
217 return std::make_reverse_iterator(begin());
218 }
219
220 const_reverse_iterator crend() const noexcept { return rend(); }
221
222 //
223 // Index iterators
224 //
225
226 iterator begin(offset_t const index) noexcept
227 {
228 return semantic::begin<N>(semantics, index);
229 }
230
231 const_iterator begin(offset_t const index) const noexcept
232 {
233 return semantic::begin<N>(semantics, index);
234 }
235
236 const_iterator cbegin(offset_t const index) const noexcept { return begin(index); }
237
238 iterator end(offset_t const index) noexcept
239 {
240 return semantic::end<N>(semantics, index);
241 }
242
243 const_iterator end(offset_t const index) const noexcept
244 {
245 return semantic::end<N>(semantics, index);
246 }
247
248 const_iterator cend(offset_t const index) const noexcept { return end(index); }
249
250 //
251 // Reverse index iterators
252 //
253
254 reverse_iterator rbegin(offset_t const index) noexcept
255 {
256 return std::make_reverse_iterator(end(index));
257 }
258
259 const_reverse_iterator rbegin(offset_t const index) const noexcept
260 {
261 return std::make_reverse_iterator(end(index));
262 }
263
264 const_reverse_iterator crbegin(offset_t const index) const noexcept
265 {
266 return rbegin(index);
267 }
268
269 reverse_iterator rend(offset_t const index) noexcept
270 {
271 return std::make_reverse_iterator(begin(index));
272 }
273
274 const_reverse_iterator rend(offset_t const index) const noexcept
275 {
276 return std::make_reverse_iterator(begin(index));
277 }
278
279 const_reverse_iterator crend(offset_t const index) const noexcept
280 {
281 return rend(index);
282 }
283
284 //
285 // Empty
286 //
287
288 [[nodiscard]] bool empty() const noexcept { return semantic::empty<N>(semantics); }
289
290 [[nodiscard]] bool empty(offset_t const index) const
291 {
292 return semantic::empty<N>(semantics, index);
293 }
294
295 //
296 // Size
297 //
298
299 [[nodiscard]] size_type size() const { return semantic::size<N>(semantics); }
300
301 [[nodiscard]] size_type size(offset_t const index) const
302 {
303 return semantic::size<N>(semantics, index);
304 }
305
306 [[nodiscard]] std::array<size_type, N> sizes() const
307 {
308 return semantic::sizes<N>(semantics);
309 }
310
311 //
312 // Resize
313 //
314
315 // resize data container without copying existing elements to the correct location
316 void resizeLazy(std::array<size_type, N> const& new_sizes)
317 {
318 semantic::resizeLazy<N>(semantics, new_sizes);
319 }
320
321 //
322 // Offset
323 //
324
325 [[nodiscard]] std::size_t offset(offset_t const index) const
326 {
327 return semantic::offset<N>(semantics, index);
328 }
329
330 //
331 // Clear
332 //
333
334 void clear() noexcept { semantic::clear<N>(semantics); }
335
336 void clear(offset_t const index) { semantic::clear<N>(semantics, index); }
337
338 //
339 // Set
340 //
341
342 void set(SemanticSet const& semantics)
343 {
344 semantic::resizeLazy<N>(this->semantics, semantics.size());
345 auto first = std::begin(semantics);
346 auto last = std::end(semantics);
347 for (offset_t i = 0; N != i; ++i) {
348 std::copy(first, last, begin(i));
349 }
350 }
351
352 void set(offset_t index, SemanticSet const& semantics)
353 {
354 semantic::resize<N>(this->semantics, index, semantics.size());
355 std::copy(std::begin(semantics), std::end(semantics), begin(index));
356 }
357
358 template <class InputIt>
359 void set(offset_t index, InputIt first, InputIt last)
360 {
361 set(index, SemanticSet(first, last));
362
363 // semantic::resize<N>(this->semantics, index,
364 // static_cast<size_type>(std::distance(first, last)));
365 // // FIXME: assumes first and last is sorted, should it?
366 // std::copy(first, last, begin(index));
367 }
368
369 //
370 // TODO: Change label
371 //
372
373 // void changeLabel(label_t old_label, label_t new_label)
374 // {
375 // // TODO: Implement
376 // }
377
378 // void changeLabel(offset_t index, label_t old_label, label_t new_label)
379 // {
380 // // TODO: Implement
381 // }
382
383 //
384 // Insert
385 //
386
387 void insert(label_t label, value_t value)
388 {
389 semantic::insert<N>(semantics, label, value);
390 }
391
392 void insert(offset_t index, label_t label, value_t value)
393 {
394 semantic::insert<N>(semantics, index, label, value);
395 }
396
397 template <class InputIt,
398 typename = std::enable_if_t<std::is_base_of_v<
399 std::input_iterator_tag,
400 typename std::iterator_traits<InputIt>::iterator_category>>>
401 void insert(InputIt first, InputIt last)
402 {
403 semantic::insert<N>(semantics, first, last);
404 }
405
406 template <class InputIt,
407 typename = std::enable_if_t<std::is_base_of_v<
408 std::input_iterator_tag,
409 typename std::iterator_traits<InputIt>::iterator_category>>>
410 void insert(offset_t index, InputIt first, InputIt last)
411 {
412 semantic::insert<N>(semantics, index, first, last);
413 }
414
415 //
416 // Insert or assign
417 //
418
419 void insertOrAssign(label_t label, value_t value)
420 {
421 semantic::insertOrAssign<N>(semantics, label, value);
422 }
423
424 void insertOrAssign(offset_t index, label_t label, value_t value)
425 {
426 semantic::insertOrAssign<N>(semantics, index, label, value);
427 }
428
429 template <class UnaryFunction,
430 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
431 void insertOrAssign(label_t label, UnaryFunction f)
432 {
433 semantic::insertOrAssign<N>(semantics, label, f);
434 }
435
436 template <class UnaryFunction,
437 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
438 void insertOrAssign(offset_t index, label_t label, UnaryFunction f)
439 {
440 semantic::insertOrAssign<N>(semantics, index, label, f);
441 }
442
443 // iterator to semantics
444 template <class InputIt,
445 typename = std::enable_if_t<std::is_base_of_v<
446 std::input_iterator_tag,
447 typename std::iterator_traits<InputIt>::iterator_category>>>
448 void insertOrAssign(InputIt first, InputIt last)
449 {
450 semantic::insertOrAssign<N>(semantics, first, last);
451 }
452
453 // iterator to semantics
454 template <class InputIt,
455 typename = std::enable_if_t<std::is_base_of_v<
456 std::input_iterator_tag,
457 typename std::iterator_traits<InputIt>::iterator_category>>>
458 void insertOrAssign(offset_t index, InputIt first, InputIt last)
459 {
460 semantic::insertOrAssign<N>(semantics, index, first, last);
461 }
462
463 // TODO: enable if InputIt is iterator, otherwise this function is called instead of
464 // insertOrAssign(index, label, f) iterator to label
465 template <class InputIt, class UnaryFunction,
466 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>,
467 typename = std::enable_if_t<std::is_base_of_v<
468 std::input_iterator_tag,
469 typename std::iterator_traits<InputIt>::iterator_category>>>
470 void insertOrAssign(InputIt first, InputIt last, UnaryFunction f)
471 {
472 semantic::insertOrAssign<N>(semantics, first, last, f);
473 }
474
475 // iterator to label
476 template <class InputIt, class UnaryFunction,
477 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>,
478 typename = std::enable_if_t<std::is_base_of_v<
479 std::input_iterator_tag,
480 typename std::iterator_traits<InputIt>::iterator_category>>>
481 void insertOrAssign(offset_t index, InputIt first, InputIt last, UnaryFunction f)
482 {
483 semantic::insertOrAssign<N>(semantics, index, first, last, f);
484 }
485
486 //
487 // Assign
488 //
489
490 // all
491 void assign(SemanticRange range, value_t value)
492 {
493 assign(semantics, SemanticRangeSet{range}, value);
494 }
495
496 void assign(SemanticRangeSet const& ranges, value_t value)
497 {
498 semantic::assign<N>(semantics, ranges, value);
499 }
500
501 template <class UnaryFunction,
502 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
503 void assign(SemanticRange range, UnaryFunction f)
504 {
505 assign(SemanticRangeSet{range}, f);
506 }
507
508 template <class UnaryFunction,
509 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
510 void assign(SemanticRangeSet const& ranges, UnaryFunction f)
511 {
512 semantic::assign<N>(semantics, ranges, f);
513 }
514
515 template <class UnaryPredicate, class UnaryFunction,
516 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>,
517 class = std::enable_if_t<std::is_invocable<UnaryPredicate, Semantic>::value>>
518 void assign(UnaryPredicate p, UnaryFunction f)
519 {
520 semantic::assign<N>(semantics, p, f);
521 }
522
523 // index
524 void assign(offset_t const index, SemanticRange range, value_t value)
525 {
526 assign(index, SemanticRangeSet{range}, value);
527 }
528
529 void assign(offset_t const index, SemanticRangeSet const& ranges, value_t value)
530 {
531 semantic::assign<N>(semantics, index, ranges, value);
532 }
533
534 template <class UnaryFunction,
535 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
536 void assign(offset_t const index, SemanticRangeSet const& ranges, UnaryFunction f)
537 {
538 semantic::assign<N>(semantics, index, ranges, f);
539 }
540
541 template <class UnaryFunction,
542 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
543 void assign(offset_t const index, SemanticRange range, UnaryFunction f)
544 {
545 assign(index, SemanticRangeSet{range}, f);
546 }
547
548 template <class UnaryPredicate, class UnaryFunction,
549 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>,
550 class = std::enable_if_t<std::is_invocable<UnaryPredicate, Semantic>::value>>
551 void assign(offset_t const index, UnaryPredicate p, UnaryFunction f)
552 {
553 semantic::assign<N>(semantics, index, p, f);
554 }
555
556 //
557 // Erase
558 //
559
560 iterator erase(const_iterator pos)
561 {
562 return semantic::erase<N>(semantics, pos, std::next(pos));
563 }
564
565 iterator erase(iterator pos)
566 {
567 return semantic::erase<N>(semantics, pos, std::next(pos));
568 }
569
570 // Removes the elements in the range [first, last)
571 iterator erase(const_iterator first, const_iterator last)
572 {
573 return semantic::erase<N>(semantics, first, last);
574 }
575
576 size_type erase(label_t label) { return semantic::erase<N>(semantics, label); }
577
578 size_type erase(SemanticRangeSet const& ranges)
579 {
580 return semantic::erase<N>(semantics, ranges);
581 }
582
583 size_type erase(SemanticRange range) { return erase(SemanticRangeSet{range}); }
584
585 size_type erase(offset_t const index, label_t label)
586 {
587 return semantic::erase<N>(semantics, index, label);
588 }
589
590 size_type erase(offset_t const index, SemanticRangeSet const& ranges)
591 {
592 return semantic::erase<N>(semantics, index, ranges);
593 }
594
595 size_type erase(offset_t const index, SemanticRange range)
596 {
597 return erase(index, SemanticRangeSet{range});
598 }
599
600 //
601 // Erase if
602 //
603
604 template <class UnaryPredicate,
605 class = std::enable_if_t<std::is_invocable<UnaryPredicate, Semantic>::value>>
606 size_type eraseIf(UnaryPredicate p)
607 {
608 return semantic::eraseIf<N>(semantics, p);
609 }
610
611 template <class UnaryPredicate,
612 class = std::enable_if_t<std::is_invocable<UnaryPredicate, Semantic>::value>>
613 size_type eraseIf(SemanticRangeSet const& ranges, UnaryPredicate p)
614 {
615 return semantic::eraseIf<N>(semantics, ranges, p);
616 }
617
618 template <class UnaryPredicate,
619 class = std::enable_if_t<std::is_invocable<UnaryPredicate, Semantic>::value>>
620 size_type eraseIf(SemanticRange range, UnaryPredicate p)
621 {
622 return eraseIf(SemanticRangeSet{range}, p);
623 }
624
625 template <class UnaryPredicate,
626 class = std::enable_if_t<std::is_invocable<UnaryPredicate, Semantic>::value>>
627 size_type eraseIf(offset_t const index, UnaryPredicate p)
628 {
629 return semantic::eraseIf<N>(semantics, index, p);
630 }
631
632 template <class UnaryPredicate,
633 class = std::enable_if_t<std::is_invocable<UnaryPredicate, Semantic>::value>>
634 size_type eraseIf(offset_t const index, SemanticRangeSet const& ranges,
635 UnaryPredicate p)
636 {
637 return semantic::eraseIf<N>(semantics, index, ranges, p);
638 }
639
640 template <class UnaryPredicate,
641 class = std::enable_if_t<std::is_invocable<UnaryPredicate, Semantic>::value>>
642 size_type eraseIf(offset_t const index, SemanticRange range, UnaryPredicate p)
643 {
644 return eraseIf(index, SemanticRangeSet{range}, p);
645 }
646
647 //
648 // At
649 //
650
651 std::optional<Semantic> at(offset_t index, label_t label) const
652 {
653 return semantic::at<N>(semantics, index, label);
654 }
655
656 //
657 // Value
658 //
659
660 std::optional<value_t> value(offset_t index, label_t label) const
661 {
662 return semantic::value<N>(semantics, index, label);
663 }
664
665 //
666 // Count
667 //
668
669 size_type count(offset_t index, label_t label) const
670 {
671 return semantic::count<N>(semantics, index, label);
672 }
673
674 //
675 // Find
676 //
677
678 const_iterator find(offset_t index, label_t label) const
679 {
680 return semantic::find<N>(semantics, index, label);
681 }
682
683 //
684 // Contains
685 //
686
687 bool contains(offset_t index, label_t label) const
688 {
689 return semantic::contains<N>(semantics, index, label);
690 }
691
692 //
693 // Equal range
694 //
695
696 std::pair<const_iterator, const_iterator> equal_range(offset_t index,
697 label_t label) const
698 {
699 return semantic::equal_range<N>(semantics, index, label);
700 }
701
702 //
703 // Lower bound
704 //
705
706 [[nodiscard]] const_iterator lower_bound(offset_t index, label_t label) const
707 {
708 return semantic::lower_bound<N>(semantics, index, label);
709 }
710
711 //
712 // Upper bound
713 //
714
715 [[nodiscard]] const_iterator upper_bound(offset_t index, label_t label) const
716 {
717 return semantic::upper_bound<N>(semantics, index, label);
718 }
719
720 //
721 // All
722 //
723
724 [[nodiscard]] bool all(offset_t index, SemanticRange range) const
725 {
726 return semantic::all<N>(semantics, index, range);
727 }
728
729 [[nodiscard]] bool all(offset_t index, SemanticRangeSet const& ranges) const
730 {
731 return semantic::all<N>(semantics, index, ranges);
732 }
733
734 template <class UnaryPredicate>
735 [[nodiscard]] bool all(offset_t index, UnaryPredicate p) const
736 {
737 return semantic::all<N>(semantics, index, p);
738 }
739
740 template <class UnaryPredicate>
741 [[nodiscard]] bool all(offset_t index, SemanticRange range, UnaryPredicate p) const
742 {
743 return semantic::all<N>(semantics, index, range, p);
744 }
745
746 template <class UnaryPredicate>
747 [[nodiscard]] bool all(offset_t index, SemanticRangeSet const& ranges,
748 UnaryPredicate p) const
749 {
750 return semantic::all<N>(semantics, index, ranges, p);
751 }
752
753 //
754 // Any
755 //
756
757 [[nodiscard]] bool any(offset_t index, SemanticRange range) const
758 {
759 return semantic::any<N>(semantics, index, range);
760 }
761
762 [[nodiscard]] bool any(offset_t index, SemanticRangeSet const& ranges) const
763 {
764 return semantic::any<N>(semantics, index, ranges);
765 }
766
767 template <class UnaryPredicate>
768 [[nodiscard]] bool any(offset_t index, UnaryPredicate p) const
769 {
770 return semantic::any<N>(semantics, index, p);
771 }
772
773 template <class UnaryPredicate>
774 [[nodiscard]] bool any(offset_t index, SemanticRange range, UnaryPredicate p) const
775 {
776 return semantic::any<N>(semantics, index, range, p);
777 }
778
779 template <class UnaryPredicate>
780 [[nodiscard]] bool any(offset_t index, SemanticRangeSet const& ranges,
781 UnaryPredicate p) const
782 {
783 return semantic::any<N>(semantics, index, ranges, p);
784 }
785
786 //
787 // None
788 //
789
790 [[nodiscard]] bool none(offset_t index, SemanticRange range) const
791 {
792 return semantic::none<N>(semantics, index, range);
793 }
794
795 [[nodiscard]] bool none(offset_t index, SemanticRangeSet const& ranges) const
796 {
797 return semantic::none<N>(semantics, index, ranges);
798 }
799
800 template <class UnaryPredicate>
801 [[nodiscard]] bool none(offset_t index, UnaryPredicate p) const
802 {
803 return semantic::none<N>(semantics, index, p);
804 }
805
806 template <class UnaryPredicate>
807 [[nodiscard]] bool none(offset_t index, SemanticRange range, UnaryPredicate p) const
808 {
809 return semantic::none<N>(semantics, index, range, p);
810 }
811
812 template <class UnaryPredicate>
813 [[nodiscard]] bool none(offset_t index, SemanticRangeSet const& ranges,
814 UnaryPredicate p) const
815 {
816 return semantic::none<N>(semantics, index, ranges, p);
817 }
818
819 std::string toString() const { return semantic::toString<N>(semantics); }
820 std::string toString(offset_t index) const
821 {
822 return semantic::toString<N>(semantics, index);
823 }
824
825 //
826 // Memory usage
827 //
828
829 [[nodiscard]] std::size_t memoryUsage() const
830 {
831 if (semantic::empty<N>(semantics)) {
832 return 0;
833 } else {
834 auto sizes = semantic::sizes<N>(semantics);
835 auto s = std::accumulate(sizes.begin(), sizes.end(), N_H);
836 return s * sizeof(Semantic);
837 }
838 }
839
840 // [[nodiscard]] std::size_t memoryUsage(offset_t index) const
841 // {
842 // return semantic::empty<N>(semantics, index) ? 0 : semantic::size<N>(semantics,
843 // index) * sizeof(Semantic);
844 // }
845
846 //
847 // Input/output (read/write)
848 //
849
850 void read(ReadBuffer& in)
851 {
852 // read number of semantics in this node
853 std::uint64_t s;
854 in.read(&s, sizeof(s));
855
856 // resize container accordingly
857 std::array<size_type, N> sizes{};
858 sizes[0] = s;
859 resizeLazy(sizes);
860
861 if (s) {
862 // copy data directly to container
863 in.read(semantics.get(), memoryUsage());
864 }
865 }
866
867 void read(ReadBuffer& in, BitSet<N> rf)
868 {
869 if (rf.all()) {
870 read(in);
871 return;
872 }
873
874 SemanticSet temp;
875 temp.read(in);
876 auto cur_sizes = sizes();
877 auto new_sizes = temp.sizes();
878 for (std::size_t i{}; N != i; ++i) {
879 new_sizes[i] = rf[i] ? new_sizes[i] : cur_sizes[i];
880 }
881
882 resizeLazy(new_sizes);
883
884 for (std::size_t i{}; N != i; ++i) {
885 if (rf[i]) {
886 std::copy(temp.begin(i), temp.end(i), begin(i));
887 }
888 }
889 }
890
891 void read(ReadBuffer& in, offset_t pos)
892 {
893 std::uint64_t s;
894 in.read(&s, sizeof(s));
895 semantic::resize<N>(semantics, pos, s);
896 if (s) {
897 in.read(semantic::begin<N>(semantics, pos), memoryUsage(pos));
898 }
899 }
900
901 void write(WriteBuffer& out) const
902 {
903 auto s = size();
904 out.write(&s, sizeof(s));
905 if (s) {
906 out.write(semantics.get(), memoryUsage());
907 }
908 }
909
910 void write(WriteBuffer& out, BitSet<N> wf)
911 {
912 // TODO: Implement
913 }
914
915 void write(WriteBuffer& out, offset_t index) const
916 {
917 std::uint64_t s = size(index);
918 out.write(&s, sizeof(s));
919 if (s) {
920 out.write(begin(index), memoryUsage(index));
921 }
922 }
923};
924
925template <>
927{
928 public:
929 // Tags
930 using value_type = Semantic;
931 // using size_type = std::size_t;
932 using size_type = label_t;
933 using difference_type = std::ptrdiff_t;
934 using reference = Semantic&; // TODO: Make label const
935 using const_reference = Semantic const&;
936 using pointer = Semantic*;
937 using const_pointer = Semantic const*;
938 using iterator = Semantic*; // TODO: Make label const
939 using const_iterator = Semantic const*;
940 using reverse_iterator = std::reverse_iterator<iterator>;
941 using const_reverse_iterator = std::reverse_iterator<const_iterator>;
942
943 //
944 // Constructors
945 //
946
947 constexpr SemanticSet() = default;
948
949 SemanticSet(SemanticSet const& other) { *this = other; }
950
951 SemanticSet(SemanticSet&& other) noexcept = default;
952
953 // SemanticSet(SemanticsReference other)
954 // {
955 // resize(other.size());
956 // std::copy(std::begin(other), std::end(other), begin());
957 // }
958
959 template <class InputIt>
960 SemanticSet(InputIt first, InputIt last)
961 {
962 insert(first, last);
963 }
964
965 SemanticSet(std::initializer_list<Semantic> init)
966 : SemanticSet(std::begin(init), std::end(init))
967 {
968 }
969
970 //
971 // Assignment operator
972 //
973
974 SemanticSet& operator=(SemanticSet const& rhs)
975 {
976 if (rhs.empty()) {
977 clear();
978 } else {
979 resize(rhs.size());
980 std::copy(std::begin(rhs), std::end(rhs), begin());
981 }
982 return *this;
983 }
984
985 SemanticSet& operator=(SemanticSet&& rhs) noexcept = default;
986
987 // SemanticSet &operator=(SemanticsReference rhs)
988 // {
989 // resize(rhs.size());
990 // std::copy(std::begin(rhs), std::end(rhs), begin());
991 // }
992
993 //
994 // Data
995 //
996
997 [[nodiscard]] const_pointer data() const { return empty() ? nullptr : data_.get() + 1; }
998
999 //
1000 // Iterators
1001 //
1002
1003 iterator begin() noexcept { return empty() ? nullptr : data_.get() + 1; }
1004
1005 const_iterator begin() const noexcept { return empty() ? nullptr : data_.get() + 1; }
1006
1007 const_iterator cbegin() const noexcept { return begin(); }
1008
1009 iterator end() noexcept { return empty() ? nullptr : data_.get() + allocSize(); }
1010
1011 const_iterator end() const noexcept
1012 {
1013 return empty() ? nullptr : data_.get() + allocSize();
1014 }
1015
1016 const_iterator cend() const noexcept { return end(); }
1017
1018 //
1019 // Reverse iterators
1020 //
1021
1022 reverse_iterator rbegin() noexcept { return std::make_reverse_iterator(end()); }
1023
1024 const_reverse_iterator rbegin() const noexcept
1025 {
1026 return std::make_reverse_iterator(end());
1027 }
1028
1029 const_reverse_iterator crbegin() const noexcept { return rbegin(); }
1030
1031 reverse_iterator rend() noexcept { return std::make_reverse_iterator(begin()); }
1032
1033 const_reverse_iterator rend() const noexcept
1034 {
1035 return std::make_reverse_iterator(begin());
1036 }
1037
1038 const_reverse_iterator crend() const noexcept { return rend(); }
1039
1040 //
1041 // Empty
1042 //
1043
1044 [[nodiscard]] bool empty() const noexcept { return nullptr == data_; }
1045
1046 //
1047 // Size
1048 //
1049
1050 [[nodiscard]] size_type size() const { return empty() ? 0 : data_[0].label; }
1051
1052 [[nodiscard]] static constexpr size_type maxSize() noexcept
1053 {
1054 return std::numeric_limits<label_t>::max();
1055 }
1056
1057 [[nodiscard]] size_type allocSize() const { return empty() ? 0 : size() + 1; }
1058
1059 //
1060 // At
1061 //
1062
1063 [[nodiscard]] std::optional<Semantic> at(label_t label) const
1064 {
1065 return semantic::at<1>(data_, 0, label);
1066 }
1067
1068 //
1069 // Value
1070 //
1071
1072 [[nodiscard]] std::optional<value_t> value(label_t label) const
1073 {
1074 return semantic::value<1>(data_, 0, label);
1075 }
1076
1077 //
1078 // Count
1079 //
1080
1081 [[nodiscard]] size_type count(label_t label) const
1082 {
1083 return semantic::count<1>(data_, 0, label);
1084 }
1085
1086 //
1087 // Find
1088 //
1089
1090 [[nodiscard]] iterator find(label_t label)
1091 {
1092 return semantic::find<1>(data_, 0, label);
1093 }
1094
1095 [[nodiscard]] const_iterator find(label_t label) const
1096 {
1097 return semantic::find<1>(data_, 0, label);
1098 }
1099
1100 //
1101 // Contains
1102 //
1103
1104 [[nodiscard]] bool contains(label_t label) const
1105 {
1106 return semantic::contains<1>(data_, 0, label);
1107 }
1108
1109 //
1110 // Equal range
1111 //
1112
1113 [[nodiscard]] std::pair<iterator, iterator> equal_range(label_t label)
1114 {
1115 return semantic::equal_range<1>(data_, 0, label);
1116 }
1117
1118 [[nodiscard]] std::pair<const_iterator, const_iterator> equal_range(label_t label) const
1119 {
1120 return semantic::equal_range<1>(data_, 0, label);
1121 }
1122
1123 //
1124 // Lower bound
1125 //
1126
1127 [[nodiscard]] iterator lower_bound(label_t label)
1128 {
1129 return semantic::lower_bound<1>(data_, 0, label);
1130 }
1131
1132 [[nodiscard]] const_iterator lower_bound(label_t label) const
1133 {
1134 return semantic::lower_bound<1>(data_, 0, label);
1135 }
1136
1137 //
1138 // Upper bound
1139 //
1140
1141 [[nodiscard]] iterator upper_bound(label_t label)
1142 {
1143 return semantic::upper_bound<1>(data_, 0, label);
1144 }
1145
1146 [[nodiscard]] const_iterator upper_bound(label_t label) const
1147 {
1148 return semantic::upper_bound<1>(data_, 0, label);
1149 }
1150
1151 //
1152 // All
1153 //
1154
1155 [[nodiscard]] bool all(SemanticRange range) const
1156 {
1157 return all(SemanticRangeSet(range));
1158 }
1159
1160 [[nodiscard]] bool all(SemanticRangeSet const& ranges) const
1161 {
1162 return semantic::all<1>(data_, 0, ranges);
1163 }
1164
1165 template <class UnaryPredicate>
1166 [[nodiscard]] bool all(UnaryPredicate p) const
1167 {
1168 return semantic::all<1>(data_, 0, p);
1169 }
1170
1171 template <class UnaryPredicate>
1172 [[nodiscard]] bool all(SemanticRange range, UnaryPredicate p) const
1173 {
1174 return all(SemanticRangeSet(range), p);
1175 }
1176
1177 template <class UnaryPredicate>
1178 [[nodiscard]] bool all(SemanticRangeSet const& ranges, UnaryPredicate p) const
1179 {
1180 return semantic::all<1>(data_, 0, ranges, p);
1181 }
1182
1183 //
1184 // Any
1185 //
1186
1187 [[nodiscard]] bool any(SemanticRange range) const
1188 {
1189 return any(SemanticRangeSet(range));
1190 }
1191
1192 [[nodiscard]] bool any(SemanticRangeSet const& ranges) const
1193 {
1194 return semantic::any<1>(data_, 0, ranges);
1195 }
1196
1197 template <class UnaryPredicate>
1198 [[nodiscard]] bool any(UnaryPredicate p) const
1199 {
1200 return semantic::any<1>(data_, 0, p);
1201 }
1202
1203 template <class UnaryPredicate>
1204 [[nodiscard]] bool any(SemanticRange range, UnaryPredicate p) const
1205 {
1206 return any(SemanticRangeSet(range), p);
1207 }
1208
1209 template <class UnaryPredicate>
1210 [[nodiscard]] bool any(SemanticRangeSet const& ranges, UnaryPredicate p) const
1211 {
1212 return semantic::any<1>(data_, 0, ranges, p);
1213 }
1214
1215 //
1216 // None
1217 //
1218
1219 [[nodiscard]] bool none(SemanticRange range) const
1220 {
1221 return none(SemanticRangeSet(range));
1222 }
1223
1224 [[nodiscard]] bool none(SemanticRangeSet const& ranges) const
1225 {
1226 return semantic::none<1>(data_, 0, ranges);
1227 }
1228
1229 template <class UnaryPredicate>
1230 [[nodiscard]] bool none(UnaryPredicate p) const
1231 {
1232 return semantic::none<1>(data_, 0, p);
1233 }
1234
1235 template <class UnaryPredicate>
1236 [[nodiscard]] bool none(SemanticRange range, UnaryPredicate p) const
1237 {
1238 return none(SemanticRangeSet(range), p);
1239 }
1240
1241 template <class UnaryPredicate>
1242 [[nodiscard]] bool none(SemanticRangeSet const& ranges, UnaryPredicate p) const
1243 {
1244 return semantic::none<1>(data_, 0, ranges, p);
1245 }
1246
1247 //
1248 // TODO: Some
1249 //
1250
1251 //
1252 // Clear
1253 //
1254
1255 void clear() noexcept { data_.reset(); }
1256
1257 //
1258 // Insert
1259 //
1260
1261 std::pair<iterator, bool> insert(Semantic semantic)
1262 {
1263 return insert(semantic.label, semantic.value);
1264 }
1265
1266 std::pair<iterator, bool> insert(label_t label, value_t value)
1267 {
1268 return semantic::insert<1>(data_, 0, label, value);
1269 }
1270
1271 std::pair<iterator, bool> insert(const_iterator hint, Semantic semantic)
1272 {
1273 return insert(hint, semantic.label, semantic.value);
1274 }
1275
1276 std::pair<iterator, bool> insert(const_iterator hint, label_t label, value_t value)
1277 {
1278 return semantic::insert<1>(data_, 0, hint, label, value);
1279 }
1280
1281 template <class InputIt>
1282 void insert(InputIt first, InputIt last)
1283 {
1284 semantic::insert<1>(data_, 0, first, last);
1285 }
1286
1287 void insert(std::initializer_list<Semantic> ilist)
1288 {
1289 insert(std::cbegin(ilist), std::cend(ilist));
1290 }
1291
1292 //
1293 // Insert or assign
1294 //
1295
1296 std::pair<iterator, bool> insertOrAssign(Semantic semantic)
1297 {
1298 return insertOrAssign(semantic.label, semantic.value);
1299 }
1300
1301 std::pair<iterator, bool> insertOrAssign(label_t label, value_t value)
1302 {
1303 return semantic::insertOrAssign<1>(data_, 0, label, value);
1304 }
1305
1306 std::pair<iterator, bool> insertOrAssign(const_iterator hint, Semantic semantic)
1307 {
1308 return insertOrAssign(hint, semantic.label, semantic.value);
1309 }
1310
1311 std::pair<iterator, bool> insertOrAssign(const_iterator hint, label_t label,
1312 value_t value)
1313 {
1314 return semantic::insertOrAssign<1>(data_, 0, hint, label, value);
1315 }
1316
1317 template <class InputIt>
1318 void insertOrAssign(InputIt first, InputIt last)
1319 {
1320 semantic::insertOrAssign<1>(data_, 0, first, last);
1321 }
1322
1323 void insertOrAssign(std::initializer_list<Semantic> ilist)
1324 {
1325 insertOrAssign(std::cbegin(ilist), std::cend(ilist));
1326 }
1327
1328 //
1329 // Insert or assign custom function
1330 //
1331
1332 template <class UnaryFunction,
1333 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
1334 void insertOrAssign(label_t label, UnaryFunction f)
1335 {
1336 semantic::insertOrAssign<1>(data_, 0, label, f);
1337 }
1338
1339 // InputIt to label_t
1340 template <class InputIt, class UnaryFunction,
1341 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
1342 void insertOrAssign(InputIt first, InputIt last, UnaryFunction f)
1343 {
1344 semantic::insertOrAssign<1>(data_, 0, first, last, f);
1345 }
1346
1347 template <class UnaryFunction,
1348 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
1349 void insertOrAssign(std::initializer_list<label_t> ilist, UnaryFunction f)
1350 {
1351 insertOrAssign(std::cbegin(ilist), std::cend(ilist), f);
1352 }
1353
1354 //
1355 // Assign
1356 //
1357
1358 void assign(SemanticRange range, value_t value)
1359 {
1360 assign(SemanticRangeSet{range}, value);
1361 }
1362
1363 void assign(SemanticRangeSet const& ranges, value_t value)
1364 {
1365 semantic::assign<1>(data_, 0, ranges, value);
1366 }
1367
1368 template <class UnaryFunction,
1369 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
1370 void assign(SemanticRange range, UnaryFunction f)
1371 {
1372 semantic::assign<1>(data_, 0, SemanticRangeSet{range}, f);
1373 }
1374
1375 template <class UnaryPredicate>
1376 void assign(UnaryPredicate p, value_t value)
1377 {
1378 assign(p, [value](auto) { return value; });
1379 }
1380
1381 template <class UnaryFunction,
1382 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
1383 void assign(SemanticRangeSet const& ranges, UnaryFunction f)
1384 {
1385 semantic::assign<1>(data_, 0, ranges, f);
1386 }
1387
1388 template <class UnaryPredicate, class UnaryFunction,
1389 class = std::enable_if_t<std::is_invocable<UnaryFunction, Semantic>::value>>
1390 void assign(UnaryPredicate p, UnaryFunction f)
1391 {
1392 semantic::assign<1>(data_, 0, p, f);
1393 }
1394
1395 //
1396 // Erase
1397 //
1398
1399 iterator erase(const_iterator pos)
1400 {
1401 return semantic::erase<1>(data_, pos, std::next(pos));
1402 }
1403
1404 iterator erase(iterator pos) { return semantic::erase<1>(data_, pos, std::next(pos)); }
1405
1406 iterator erase(const_iterator first, const_iterator last)
1407 {
1408 return semantic::erase<1>(data_, first, last);
1409 }
1410
1411 size_type erase(label_t label) { return semantic::erase<1>(data_, 0, label); }
1412
1413 size_type erase(SemanticRangeSet const& ranges)
1414 {
1415 return semantic::erase<1>(data_, 0, ranges);
1416 }
1417
1418 size_type erase(SemanticRange range) { return erase(SemanticRangeSet{range}); }
1419
1420 //
1421 // Erase if
1422 //
1423
1424 template <class UnaryPredicate>
1425 size_type eraseIf(UnaryPredicate p)
1426 {
1427 return semantic::eraseIf<1>(data_, 0, p);
1428 }
1429
1430 template <class UnaryPredicate>
1431 size_type eraseIf(SemanticRangeSet const& ranges, UnaryPredicate p)
1432 {
1433 return semantic::eraseIf<1>(data_, 0, ranges, p);
1434 }
1435
1436 template <class UnaryPredicate>
1437 size_type eraseIf(SemanticRange range, UnaryPredicate p)
1438 {
1439 return eraseIf(SemanticRangeSet{range}, p);
1440 }
1441
1442 //
1443 // Swap
1444 //
1445
1446 void swap(SemanticSet& other) noexcept { std::swap(data_, other.data_); }
1447
1448 std::string toString() const { return semantic::toString<1>(data_); }
1449
1450 protected:
1451 //
1452 // Data
1453 //
1454
1455 [[nodiscard]] pointer data() { return empty() ? nullptr : data_.get() + 1; }
1456
1457 //
1458 // Resize
1459 //
1460
1461 void resize(size_type size)
1462 {
1463 if (0 == size) {
1464 clear();
1465 return;
1466 } else if (this->size() == size) {
1467 return;
1468 }
1469
1470 pointer p_cur = data_.release();
1471 pointer p_new = static_cast<pointer>(realloc(p_cur, (size + 1) * sizeof(Semantic)));
1472
1473 if (!p_new) {
1474 data_.reset(p_cur);
1475 throw std::bad_alloc();
1476 }
1477
1478 data_.reset(p_new);
1479 data_[0].label = static_cast<label_t>(size); // TODO: is this cast ok?
1480 }
1481
1482 private:
1483 std::unique_ptr<Semantic[]> data_;
1484};
1485} // namespace ufo
1486
1487namespace std
1488{
1489template <std::size_t N>
1490inline void swap(ufo::SemanticSet<N>& lhs,
1491 ufo::SemanticSet<N>& rhs) noexcept(noexcept(lhs.swap(rhs)))
1492{
1493 lhs.swap(rhs);
1494}
1495
1496template <std::size_t N>
1497inline std::ostream& operator<<(std::ostream& out, ufo::SemanticSet<N> const& s)
1498{
1499 return out << s.toString();
1500}
1501} // namespace std
1502
1503#endif // UFO_MAP_SEMANTICS_H
STL namespace.
All vision-related classes and functions.
Definition cloud.hpp:49
Represents a semantic annotation as a pair of class label and confidence score.
Definition semantic.hpp:75
Label label
The discrete semantic class identifier.
Definition semantic.hpp:79