UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
query_iterator.hpp
1
42#ifndef UFO_CONTAINER_TREE_SET_QUERY_ITERATOR_HPP
43#define UFO_CONTAINER_TREE_SET_QUERY_ITERATOR_HPP
44
45// UFO
46#include <ufo/container/tree/index.hpp>
47#include <ufo/container/tree/predicate/filter.hpp>
48#include <ufo/container/tree/predicate/predicate.hpp>
49
50// STL
51#include <cassert>
52#include <cstddef>
53#include <iterator>
54
55namespace ufo
56{
57// Forward declare
58template <std::size_t Dim>
59class TreeSet;
60
61template <bool Const, std::size_t Dim, class Predicate = pred::Predicate<TreeSet<Dim>>>
63{
64 private:
65 //
66 // Friends
67 //
68
69 template <bool, std::size_t, class>
70 friend class TreeSetQueryIterator;
71
72 friend class TreeSet<Dim>;
73
74 private:
75 static constexpr std::size_t const BF = TreeSet<Dim>::branchingFactor();
76
77 using RawIterator =
78 std::conditional_t<Const, typename TreeSet<Dim>::container_type::const_iterator,
80
82
83 public:
84 //
85 // Tags
86 //
87
88 using iterator_category = std::forward_iterator_tag;
89 using difference_type = std::ptrdiff_t;
90 using value_type = typename std::iterator_traits<RawIterator>::value_type;
91 using reference = typename std::iterator_traits<RawIterator>::reference;
92 using pointer = typename std::iterator_traits<RawIterator>::pointer;
93
94 TreeSetQueryIterator() = default;
95
97 : ts_(other.ts_)
98 , pred_(other.pred_)
99 , root_(other.root_)
100 , cur_(other.cur_)
101 , it_(other.it_)
102 , last_(other.last_)
103 {
104 }
105
106 // From non-const to const or change of predicate type
107 template <
108 bool Const2, class Predicate2,
109 std::enable_if_t<(Const && !Const2) ||
110 (Const == Const2 && !std::is_same_v<Predicate, Predicate2>),
111 bool> = true>
113 : ts_(other.ts_)
114 , pred_(other.pred_)
115 , root_(other.root_)
116 , cur_(other.cur_)
117 , it_(other.it_)
118 , last_(other.last_)
119 {
120 }
121
122 TreeSetQueryIterator& operator++()
123 {
124 ++it_;
125 while (!nextValue()) {
126 if (!nextNode()) {
127 break;
128 }
129 }
130 return *this;
131 }
132
133 TreeSetQueryIterator operator++(int)
134 {
135 TreeSetQueryIterator tmp(*this);
136 ++*this;
137 return tmp;
138 }
139
140 reference operator*() const { return *it_; }
141
142 pointer operator->() const { return &*it_; }
143
144 template <bool Const2, class Predicate2>
145 bool operator==(TreeSetQueryIterator<Const2, Dim, Predicate2> const& other)
146 {
147 return it_ == other.it_;
148 }
149
150 template <bool Const2, class Predicate2>
151 bool operator!=(TreeSetQueryIterator<Const2, Dim, Predicate2> const& other)
152 {
153 return !(*this == other);
154 }
155
156 private:
157 [[nodiscard]] bool returnable(value_type const& value) const
158 {
159 return Filter::returnable(pred_, value);
160 }
161
162 [[nodiscard]] bool returnable(TreeIndex node) const
163 {
164 return ts_->isPureLeaf(node) && !ts_->empty(node);
165 // FIXME: && Filter::returnable(pred_, *ts_, ts_->node(node));
166 // FIXME: && Filter::traversable(pred_, *ts_, ts_->node(node));
167 }
168
169 [[nodiscard]] bool traversable(TreeIndex node) const
170 {
171 return ts_->isParent(node) && Filter::traversable(pred_, *ts_, ts_->node(node));
172 }
173
179 bool nextValue()
180 {
181 it_ = std::find_if(it_, last_, [this](auto const& v) { return returnable(v); });
182 return it_ != last_;
183 }
184
190 bool nextNode()
191 {
192 while (root_ != cur_) {
193 if (BF - 1 == cur_.offset) {
194 cur_ = ts_->parent(cur_);
195 continue;
196 }
197
198 ++cur_.offset;
199
200 if (nextNodeDownwards()) {
201 return true;
202 }
203 }
204
205 // We have visited all nodes
206 it_ = {};
207 last_ = {};
208 return false;
209 }
210
216 bool nextNodeDownwards()
217 {
218 while (true) {
219 if (returnable(cur_)) {
220 it_ = ts_->values(cur_).begin();
221 last_ = ts_->values(cur_).end();
222 return true;
223 } else if (traversable(cur_)) {
224 cur_ = ts_->child(cur_, 0);
225 } else {
226 break;
227 }
228 }
229 return false;
230 }
231
232 [[nodiscard]] RawIterator iterator() { return it_; }
233
234 private:
235 TreeSetQueryIterator(TreeSet<Dim>* ts, TreeIndex node, Predicate const& pred)
236 : ts_(ts), pred_(pred), root_(node), cur_(node)
237 {
238 Filter::init(pred_, *ts_);
239
240 nextNodeDownwards();
241 while (!nextValue()) {
242 if (!nextNode()) {
243 break;
244 }
245 }
246 }
247
248 // From const to non-const
249 template <bool Const2, class Predicate2,
250 std::enable_if_t<!Const && Const2, bool> = true>
252 : ts_(other.ts_), pred_(other.pred_), root_(other.root_), cur_(other.cur_)
253 {
254 // Remove const from other.it_ and other.last_
255 it_ = ts_->values(cur_).erase(other.it_, other.it_);
256 last_ = ts_->values(cur_).erase(other.last_, other.last_);
257 }
258
259 private:
260 TreeSet<Dim>* ts_ = nullptr;
261
262 Predicate pred_{};
263
264 TreeIndex root_{};
265 TreeIndex cur_{};
266
267 RawIterator it_{};
268 RawIterator last_{};
269};
270} // namespace ufo
271
272#endif // UFO_CONTAINER_TREE_SET_QUERY_ITERATOR_HPP
All vision-related classes and functions.
Definition cloud.hpp:49