UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
reflection_map.hpp
1
42#ifndef UFO_MAP_REFLECTION_MAP_HPP
43#define UFO_MAP_REFLECTION_MAP_HPP
44
45// UFO
46#include <ufo/map/reflection/is_reflection_map.hpp>
47#include <ufo/map/reflection/reflection.hpp>
48#include <ufo/map/types.hpp>
49#include <ufo/utility/bit_set.hpp>
50#include <ufo/utility/io/buffer.hpp>
51
52// STL
53#include <algorithm>
54#include <limits>
55
56namespace ufo
57{
58template <class Derived, offset_t N, class Index, class Node, class Code, class Key,
59 class Coord>
61{
62 public:
63 using ReflectionBlock = DataBlock<Reflection, N>;
64
65 //
66 // Reflection block
67 //
68
69 [[nodiscard]] ReflectionBlock& reflectionBlock(pos_t block)
70 {
71 return reflection_[block];
72 }
73
74 [[nodiscard]] ReflectionBlock const& reflectionBlock(pos_t block) const
75 {
76 return reflection_[block];
77 }
78
79 //
80 // Get reflection
81 //
82
83 [[nodiscard]] Reflection reflection(Index node) const
84 {
85 return reflection_[node.pos][node.offset];
86 }
87
88 [[nodiscard]] Reflection reflection(Node node) const
89 {
90 return reflection(derived().index(node));
91 }
92
93 [[nodiscard]] Reflection reflection(Code code) const
94 {
95 return reflection(derived().index(code));
96 }
97
98 [[nodiscard]] Reflection reflection(Key key) const
99 {
100 return reflection(derived().index(key));
101 }
102
103 [[nodiscard]] Reflection reflection(Point coord, depth_t depth = 0) const
104 {
105 return reflection(derived().index(coord, depth));
106 }
107
108 //
109 // Get reflectiveness
110 //
111
112 [[nodiscard]] reflection_t reflectiveness(Index node) const
113 {
114 return reflection(node).reflectiveness();
115 }
116
117 [[nodiscard]] reflection_t reflectiveness(Node node) const
118 {
119 return reflectiveness(derived().index(node));
120 }
121
122 [[nodiscard]] reflection_t reflectiveness(Code code) const
123 {
124 return reflectiveness(derived().index(code));
125 }
126
127 [[nodiscard]] reflection_t reflectiveness(Key key) const
128 {
129 return reflectiveness(derived().index(key));
130 }
131
132 [[nodiscard]] reflection_t reflectiveness(Point coord, depth_t depth = 0) const
133 {
134 return reflectiveness(derived().index(coord, depth));
135 }
136
137 //
138 // Get hits
139 //
140
141 [[nodiscard]] count_t hits(Index node) const { return reflection(node).hits; }
142
143 [[nodiscard]] count_t hits(Node node) const { return hits(derived().index(node)); }
144
145 [[nodiscard]] count_t hits(Code code) const { return hits(derived().index(code)); }
146
147 [[nodiscard]] count_t hits(Key key) const { return hits(derived().index(key)); }
148
149 [[nodiscard]] count_t hits(Point coord, depth_t depth = 0) const
150 {
151 return hits(derived().index(coord, depth));
152 }
153
154 //
155 // Get misses
156 //
157
158 [[nodiscard]] count_t misses(Index node) const { return reflection(node).misses; }
159
160 [[nodiscard]] count_t misses(Node node) const { return misses(derived().index(node)); }
161
162 [[nodiscard]] count_t misses(Code code) const { return misses(derived().index(code)); }
163
164 [[nodiscard]] count_t misses(Key key) const { return misses(derived().index(key)); }
165
166 [[nodiscard]] count_t misses(Point coord, depth_t depth = 0) const
167 {
168 return misses(derived().index(coord, depth));
169 }
170
171 //
172 // Set reflection
173 //
174
175 void setReflection(Index node, Reflection value)
176 {
177 derived().apply(
178 node, [this, value](Index node) { reflection_[node.pos][node.offset] = value; },
179 [this, value](pos_t pos) { reflection_[pos].fill(value); });
180 }
181
182 Node setReflection(Node node, Reflection value, bool propagate = true)
183 {
184 return derived().apply(
185 node, [this, value](Index node) { reflection_[node.pos][node.offset] = value; },
186 [this, value](pos_t pos) { reflection_[pos].fill(value); }, propagate);
187 }
188
189 Node setReflection(Code code, Reflection value, bool propagate = true)
190 {
191 return derived().apply(
192 code, [this, value](Index node) { reflection_[node.pos][node.offset] = value; },
193 [this, value](pos_t pos) { reflection_[pos].fill(value); }, propagate);
194 }
195
196 Node setReflection(Key key, Reflection value, bool propagate = true)
197 {
198 return setReflection(derived().toCode(key), value, propagate);
199 }
200
201 Node setReflection(Point coord, Reflection value, bool propagate = true,
202 depth_t depth = 0)
203 {
204 return setReflection(derived().toCode(coord, depth), value, propagate);
205 }
206
207 //
208 // Set hits
209 //
210
211 void setHits(Index node, count_t value)
212 {
213 return derived().apply(
214 node,
215 [this, value](Index node) { reflection_[node.pos][node.offset].hits = value; },
216 [this, value](pos_t pos) {
217 for (auto& r : reflection_[pos]) {
218 r.hits = value;
219 }
220 });
221 }
222
223 Node setHits(Node node, count_t value, bool propagate = true)
224 {
225 return derived().apply(
226 node,
227 [this, value](Index node) { reflection_[node.pos][node.offset].hits = value; },
228 [this, value](pos_t pos) {
229 for (auto& r : reflection_[pos]) {
230 r.hits = value;
231 }
232 },
233 propagate);
234 }
235
236 Node setHits(Code code, count_t value, bool propagate = true)
237 {
238 return derived().apply(
239 code,
240 [this, value](Index node) { reflection_[node.pos][node.offset].hits = value; },
241 [this, value](pos_t pos) {
242 for (auto& r : reflection_[pos]) {
243 r.hits = value;
244 }
245 },
246 propagate);
247 }
248
249 Node setHits(Key key, count_t value, bool propagate = true)
250 {
251 return setHits(derived().toCode(key), value, propagate);
252 }
253
254 Node setHits(Point coord, count_t value, bool propagate = true, depth_t depth = 0)
255 {
256 return setHits(derived().toCode(coord, depth), value, propagate);
257 }
258
259 //
260 // Set misses
261 //
262
263 void setMisses(Index node, count_t value)
264 {
265 return derived().apply(
266 node,
267 [this, value](Index node) { reflection_[node.pos][node.offset].misses = value; },
268 [this, value](pos_t pos) {
269 for (auto& r : reflection_[pos]) {
270 r.misses = value;
271 }
272 });
273 }
274
275 Node setMisses(Node node, count_t value, bool propagate = true)
276 {
277 return derived().apply(
278 node,
279 [this, value](Index node) { reflection_[node.pos][node.offset].misses = value; },
280 [this, value](pos_t pos) {
281 for (auto& r : reflection_[pos]) {
282 r.misses = value;
283 }
284 },
285 propagate);
286 }
287
288 Node setMisses(Code code, count_t value, bool propagate = true)
289 {
290 return derived().apply(
291 code,
292 [this, value](Index node) { reflection_[node.pos][node.offset].misses = value; },
293 [this, value](pos_t pos) {
294 for (auto& r : reflection_[pos]) {
295 r.misses = value;
296 }
297 },
298 propagate);
299 }
300
301 Node setMisses(Key key, count_t value, bool propagate = true)
302 {
303 return setMisses(derived().toCode(key), value, propagate);
304 }
305
306 Node setMisses(Point coord, count_t value, bool propagate = true, depth_t depth = 0)
307 {
308 return setMisses(derived().toCode(coord, depth), value, propagate);
309 }
310
311 //
312 // Update reflection
313 //
314
315 void updateReflection(Index node, int hits_change, int misses_change)
316 {
317 derived().apply(
318 node,
319 [this, hits_change, misses_change](Index node) {
320 reflection_[node.pos][node.offset].hits += hits_change;
321 reflection_[node.pos][node.offset].misses += misses_change;
322 },
323 [this, hits_change, misses_change](pos_t pos) {
324 for (auto& e : reflection_[pos]) {
325 e.hits += hits_change;
326 e.misses += misses_change;
327 }
328 });
329 }
330
331 template <class UnaryOp,
332 std::enable_if_t<std::is_invocable_v<UnaryOp, Reflection>, bool> = true>
333 void updateReflection(Index node, UnaryOp unary_op)
334 {
335 derived().apply(
336 node,
337 [this, unary_op](Index node) {
338 reflection_[node.pos][node.offset] =
339 unary_op(reflection_[node.pos][node.offset]);
340 },
341 [this, unary_op](pos_t pos) {
342 for (auto& e : reflection_[pos]) {
343 e = unary_op(e);
344 }
345 });
346 }
347
348 template <
349 class BinaryOp,
350 std::enable_if_t<std::is_invocable_v<BinaryOp, Index, Reflection>, bool> = true>
351 void updateReflection(Index node, BinaryOp binary_op)
352 {
353 derived().apply(
354 node,
355 [this, binary_op](Index node) {
356 reflection_[node.pos][node.offset] =
357 binary_op(node, reflection_[node.pos][node.offset]);
358 },
359 [this, binary_op](pos_t pos) {
360 offset_t i{};
361 for (auto& e : reflection_[pos]) {
362 e = binary_op(Index(pos, i++), e);
363 }
364 });
365 }
366
367 Node updateReflection(Node node, int hits_change, int misses_change,
368 bool propagate = true)
369 {
370 return derived().apply(
371 node,
372 [this, hits_change, misses_change](Index node) {
373 reflection_[node.pos][node.offset].hits += hits_change;
374 reflection_[node.pos][node.offset].misses += misses_change;
375 },
376 [this, hits_change, misses_change](pos_t pos) {
377 for (auto& e : reflection_[pos]) {
378 e.hits += hits_change;
379 e.misses += misses_change;
380 }
381 },
382 propagate);
383 }
384
385 template <class UnaryOp,
386 std::enable_if_t<std::is_invocable_v<UnaryOp, Reflection>, bool> = true>
387 Node updateReflection(Node node, UnaryOp unary_op, bool propagate = true)
388 {
389 return derived().apply(
390 node,
391 [this, unary_op](Index node) {
392 reflection_[node.pos][node.offset] =
393 unary_op(reflection_[node.pos][node.offset]);
394 },
395 [this, unary_op](pos_t pos) {
396 for (auto& e : reflection_[pos]) {
397 e = unary_op(e);
398 }
399 },
400 propagate);
401 }
402
403 template <
404 class BinaryOp,
405 std::enable_if_t<std::is_invocable_v<BinaryOp, Index, Reflection>, bool> = true>
406 Node updateReflection(Node node, BinaryOp binary_op, bool propagate = true)
407 {
408 return derived().apply(
409 node,
410 [this, binary_op](Index node) {
411 reflection_[node.pos][node.offset] =
412 binary_op(node, reflection_[node.pos][node.offset]);
413 },
414 [this, binary_op](pos_t pos) {
415 offset_t i{};
416 for (auto& e : reflection_[pos]) {
417 e = binary_op(Index(pos, i++), e);
418 }
419 },
420 propagate);
421 }
422
423 Node updateReflection(Code code, int hits_change, int misses_change,
424 bool propagate = true)
425 {
426 return derived().apply(
427 code,
428 [this, hits_change, misses_change](Index node) {
429 reflection_[node.pos][node.offset].hits += hits_change;
430 reflection_[node.pos][node.offset].misses += misses_change;
431 },
432 [this, hits_change, misses_change](pos_t pos) {
433 for (auto& e : reflection_[pos]) {
434 e.hits += hits_change;
435 e.misses += misses_change;
436 }
437 },
438 propagate);
439 }
440
441 template <class UnaryOp,
442 std::enable_if_t<std::is_invocable_v<UnaryOp, Reflection>, bool> = true>
443 Node updateReflection(Code code, UnaryOp unary_op, bool propagate = true)
444 {
445 return derived().apply(
446 code,
447 [this, unary_op](Index node) {
448 reflection_[node.pos][node.offset] =
449 unary_op(reflection_[node.pos][node.offset]);
450 },
451 [this, unary_op](pos_t pos) {
452 for (auto& e : reflection_[pos]) {
453 e = unary_op(e);
454 }
455 },
456 propagate);
457 }
458
459 template <
460 class BinaryOp,
461 std::enable_if_t<std::is_invocable_v<BinaryOp, Index, Reflection>, bool> = true>
462 Node updateReflection(Code code, BinaryOp binary_op, bool propagate = true)
463 {
464 return derived().apply(
465 code,
466 [this, binary_op](Index node) {
467 reflection_[node.pos][node.offset] =
468 binary_op(node, reflection_[node.pos][node.offset]);
469 },
470 [this, binary_op](pos_t pos) {
471 offset_t i{};
472 for (auto& e : reflection_[pos]) {
473 e = binary_op(Index(pos, i++), e);
474 }
475 },
476 propagate);
477 }
478
479 Node updateReflection(Key key, int hits_change, int misses_change,
480 bool propagate = true)
481 {
482 return updateReflection(derived().toCode(key), hits_change, misses_change, propagate);
483 }
484
485 template <class UnaryOp,
486 std::enable_if_t<std::is_invocable_v<UnaryOp, Reflection>, bool> = true>
487 Node updateReflection(Key key, UnaryOp unary_op, bool propagate = true)
488 {
489 return updateReflection(derived().toCode(key), unary_op, propagate);
490 }
491
492 template <
493 class BinaryOp,
494 std::enable_if_t<std::is_invocable_v<BinaryOp, Index, Reflection>, bool> = true>
495 Node updateReflection(Key key, BinaryOp binary_op, bool propagate = true)
496 {
497 return updateReflection(derived().toCode(key), binary_op, propagate);
498 }
499
500 Node updateReflection(Point coord, int hits_change, int misses_change,
501 bool propagate = true, depth_t depth = 0)
502 {
503 return updateReflection(derived().toCode(coord, depth), hits_change, misses_change,
504 propagate);
505 }
506
507 template <class UnaryOp,
508 std::enable_if_t<std::is_invocable_v<UnaryOp, Reflection>, bool> = true>
509 Node updateReflection(Point coord, UnaryOp unary_op, bool propagate = true,
510 depth_t depth = 0)
511 {
512 return updateReflection(derived().toCode(coord, depth), unary_op, propagate);
513 }
514
515 template <
516 class BinaryOp,
517 std::enable_if_t<std::is_invocable_v<BinaryOp, Index, Reflection>, bool> = true>
518 Node updateReflection(Point coord, BinaryOp binary_op, bool propagate = true,
519 depth_t depth = 0)
520 {
521 return updateReflection(derived().toCode(coord, depth), binary_op, propagate);
522 }
523
524 //
525 // Propagation criteria
526 //
527
528 [[nodiscard]] constexpr PropagationCriteria reflectionPropagationCriteria()
529 const noexcept
530 {
531 return prop_criteria_;
532 }
533
534 constexpr void setReflectionPropagationCriteria(PropagationCriteria prop_criteria,
535 bool propagate = true) noexcept
536 {
537 if (prop_criteria_ == prop_criteria) {
538 return;
539 }
540
541 prop_criteria_ = prop_criteria;
542
543 derived().setModified();
544
545 if (propagate) {
546 derived().propagateModified();
547 }
548 }
549
550 protected:
551 //
552 // Constructors
553 //
554
555 ReflectionMap() { reflection_.emplace_back(); }
556
557 ReflectionMap(ReflectionMap const& other) = default;
558
559 ReflectionMap(ReflectionMap&& other) = default;
560
561 template <class Derived2>
563 : reflection_(other.reflection_), prop_criteria_(other.prop_criteria_)
564 {
565 }
566
567 template <class Derived2>
569 : reflection_(std::move(other.reflection_))
570 , prop_criteria_(std::move(other.prop_criteria_))
571 {
572 }
573
574 //
575 // Destructor
576 //
577
578 ~ReflectionMap() = default;
579
580 //
581 // Assignment operator
582 //
583
584 ReflectionMap& operator=(ReflectionMap const& rhs) = default;
585
586 ReflectionMap& operator=(ReflectionMap&& rhs) = default;
587
588 template <class Derived2>
589 ReflectionMap& operator=(
591 {
592 reflection_ = rhs.reflection_;
593 prop_criteria_ = rhs.prop_criteria_;
594 return *this;
595 }
596
597 template <class Derived2>
598 ReflectionMap& operator=(
600 {
601 reflection_ = std::move(rhs.reflection_);
602 prop_criteria_ = std::move(rhs.prop_criteria_);
603 return *this;
604 }
605
606 //
607 // Swap
608 //
609
610 void swap(ReflectionMap& other) noexcept
611 {
612 std::swap(reflection_, other.reflection_);
613 std::swap(prop_criteria_, other.prop_criteria_);
614 }
615
616 //
617 // Derived
618 //
619
620 [[nodiscard]] constexpr Derived& derived() { return *static_cast<Derived*>(this); }
621
622 [[nodiscard]] constexpr Derived const& derived() const
623 {
624 return *static_cast<Derived const*>(this);
625 }
626
627 //
628 // Create node block
629 //
630
631 void createBlock(Index node)
632 {
633 reflection_.emplace_back();
634 reflection_.back().fill(reflection_[node.pos][node.offset]);
635 }
636
637 //
638 // Resize
639 //
640
641 void resize(std::size_t count)
642 {
643 // TODO: Implement
644 reflection_.resize(count);
645 }
646
647 //
648 // Reserve
649 //
650
651 void reserveImpl(std::size_t new_cap) { reflection_.reserve(new_cap); }
652
653 //
654 // Initialize root
655 //
656
657 void initRoot()
658 {
659 auto node = derived().rootIndex();
660 reflection_[node.pos][node.offset] = {0, 0};
661 }
662
663 //
664 // Fill
665 //
666
667 void fill(Index node, pos_t children)
668 {
669 reflection_[children].fill(reflection_[node.pos][node.offset]);
670 }
671
672 //
673 // Clear
674 //
675
676 void clearImpl() { reflection_.resize(1); }
677
678 void clearImpl(pos_t) {}
679
680 //
681 // Update block
682 //
683
684 void updateBlock(pos_t block, std::array<bool, N> modified_parent)
685 {
686 for (offset_t i{}; N != i; ++i) {
687 if (modified_parent[i]) {
688 Index node(block, i);
689 updateNode(node, derived().children(node));
690 }
691 }
692 }
693
694 void updateNode(Index node, pos_t children)
695 {
696 // TODO: Add sum?
697 // TODO: What does 'MIN', 'MAX', and 'MEAN' mean?
698 switch (reflectionPropagationCriteria()) {
699 case PropagationCriteria::MIN:
700 reflection_[node.pos][node.offset] = min(children);
701 return;
702 case PropagationCriteria::MAX:
703 reflection_[node.pos][node.offset] = max(children);
704 return;
705 case PropagationCriteria::MEAN:
706 reflection_[node.pos][node.offset] = mean(children);
707 return;
708 case PropagationCriteria::NONE: return;
709 }
710 }
711
712 [[nodiscard]] constexpr Reflection min(pos_t block) const
713 {
714 Reflection res(std::numeric_limits<count_t>::max(),
715 std::numeric_limits<count_t>::max());
716 for (auto r : reflection_[block]) {
717 res.hits = std::min(res.hits, r.hits);
718 res.misses = std::min(res.misses, r.misses);
719 }
720 return res;
721 }
722
723 [[nodiscard]] constexpr Reflection max(pos_t block) const
724 {
725 Reflection res(std::numeric_limits<count_t>::lowest(),
726 std::numeric_limits<count_t>::lowest());
727 for (auto r : reflection_[block]) {
728 res.hits = std::max(res.hits, r.hits);
729 res.misses = std::max(res.misses, r.misses);
730 }
731 return res;
732 }
733
734 [[nodiscard]] constexpr Reflection mean(pos_t block) const
735 {
736 count_t hits{}, misses{};
737 for (auto r : reflection_[block]) {
738 hits += r.hits;
739 misses += r.misses;
740 }
741 return Reflection(hits / static_cast<count_t>(N), misses / static_cast<count_t>(N));
742 }
743
744 //
745 // Is prunable
746 //
747
748 [[nodiscard]] bool isPrunable(pos_t block) const
749 {
750 // TODO: Use floor(log2(X))?
751 return std::all_of(std::cbegin(reflection_[block]) + 1, std::cend(reflection_[block]),
752 [a = reflection_[block].front()](auto b) { return a == b; });
753 }
754
755 void preparePrune(Index node) {}
756
757 //
758 // Memory
759 //
760
761 [[nodiscard]] static constexpr std::size_t sizeofNodeTimesN(Index) noexcept
762 {
763 return sizeofBlockLowerBound();
764 }
765
766 [[nodiscard]] static constexpr std::size_t sizeofBlock(pos_t) noexcept
767 {
768 return sizeofBlockLowerBound();
769 }
770
771 [[nodiscard]] static constexpr std::size_t sizeofBlockLowerBound() noexcept
772 {
773 return sizeof(typename decltype(reflection_)::value_type);
774 }
775
776 [[nodiscard]] static constexpr std::size_t sizeofMap() noexcept
777 {
778 return sizeof(reflection_) + sizeof(prop_criteria_);
779 }
780
781 //
782 // Input/output (read/write)
783 //
784
785 [[nodiscard]] static constexpr MapType mapType() noexcept
786 {
787 return MapType::REFLECTION;
788 }
789
790 [[nodiscard]] static constexpr std::size_t serializedSizeBlock() noexcept
791 {
792 return sizeof(typename decltype(reflection_)::value_type);
793 }
794
795 template <class Container>
796 constexpr std::size_t serializedSize(Container const& c) const
797 {
798 return c.size() * serializedSizeBlock();
799 }
800
801 template <class Container>
802 void readNodes(ReadBuffer& in, Container const& c)
803 {
804 for (auto const [pos, offsets] : c) {
805 if (offsets.all()) {
806 in.read(reflection_[pos].data(), serializedSizeBlock());
807 } else {
808 DataBlock<reflection_t, N> reflection;
809 in.read(reflection.data(), serializedSizeBlock());
810 for (offset_t i{}; N != i; ++i) {
811 reflection_[pos][i] = offsets[i] ? reflection[i] : reflection_[pos][i];
812 }
813 }
814 }
815 }
816
817 template <class BlockRange>
818 void writeBlocks(WriteBuffer& out, BlockRange const& blocks) const
819 {
820 for (auto block : blocks) {
821 out.write(reflection_[block].data(), serializedSizeBlock());
822 }
823 }
824
825 //
826 // Dot file info
827 //
828
829 void dotFileInfo(std::ostream& out, Index node) const
830 {
831 out << reflection_[node.pos][node.offset];
832 }
833
834 protected:
835 // Data
836 Container<ReflectionBlock> reflection_;
837
838 // Propagation criteria
839 PropagationCriteria prop_criteria_ = PropagationCriteria::MAX;
840
841 template <class Derived2, offset_t N2, class Index2, class Node2, class Code2,
842 class Key2, class Coord2>
843 friend class ReflectionMap;
844};
845} // namespace ufo
846
847#endif // UFO_MAP_REFLECTION_MAP_HPP
All vision-related classes and functions.
Definition cloud.hpp:49
constexpr T b(Lab< T, Flags > color) noexcept
Returns the un-weighted blue–yellow axis value.
Definition lab.hpp:326
constexpr T a(Lab< T, Flags > color) noexcept
Returns the un-weighted green–red axis value.
Definition lab.hpp:310