/
/
/
1#include "fast_slam/MapTreeIterator.hpp"
2
3#include <algorithm>
4#include <cassert>
5
6#include "fast_slam/MapTree.hpp"
7
8namespace fastslam {
9
10// Constructs iterator and positions it at first valid landmark in range
11MapTreeRangeIterator::MapTreeRangeIterator(const MapTree* tree, uint32_t start_id, uint32_t end_id, BoundaryType start_boundary,
12 BoundaryType end_boundary)
13 : tree_(tree),
14 start_id_(start_id),
15 end_id_(end_id),
16 start_boundary_(start_boundary),
17 end_boundary_(end_boundary),
18 current_landmark_(nullptr),
19 current_id_(0),
20 is_end_(false) {
21 if (!tree_ || !tree_->getRoot() || start_id > end_id) {
22 is_end_ = true;
23 return;
24 }
25
26 findFirstInRange();
27}
28
29MapTreeRangeIterator::MapTreeRangeIterator()
30 : tree_(nullptr),
31 start_id_(0),
32 end_id_(0),
33 start_boundary_(BoundaryType::INCLUSIVE),
34 end_boundary_(BoundaryType::INCLUSIVE),
35 current_landmark_(nullptr),
36 current_id_(0),
37 is_end_(true) {}
38
39MapTreeRangeIterator& MapTreeRangeIterator::operator++() {
40 if (is_end_) {
41 return *this;
42 }
43
44 if (!findNext()) {
45 is_end_ = true;
46 current_landmark_ = nullptr;
47 current_id_ = 0;
48 }
49
50 return *this;
51}
52
53MapTreeRangeIterator MapTreeRangeIterator::operator++(int) {
54 MapTreeRangeIterator temp = *this;
55 ++(*this);
56 return temp;
57}
58
59MapTreeRangeIterator& MapTreeRangeIterator::operator--() {
60 if (!tree_) return *this;
61
62 if (is_end_) {
63 findLastInRange();
64 is_end_ = false;
65 return *this;
66 }
67
68 if (!findPrevious()) {
69 current_landmark_ = nullptr;
70 current_id_ = 0;
71 }
72
73 return *this;
74}
75
76MapTreeRangeIterator MapTreeRangeIterator::operator--(int) {
77 MapTreeRangeIterator temp = *this;
78 --(*this);
79 return temp;
80}
81
82MapTreeRangeIterator::reference MapTreeRangeIterator::operator*() const {
83 if (is_end_ || !current_landmark_) {
84 static std::shared_ptr<Landmark> null_landmark = nullptr;
85 return null_landmark;
86 }
87 return const_cast<reference>(current_landmark_);
88}
89
90MapTreeRangeIterator::pointer MapTreeRangeIterator::operator->() const { return &(operator*()); }
91
92bool MapTreeRangeIterator::operator==(const MapTreeRangeIterator& other) const {
93 if (is_end_ && other.is_end_) return true;
94 if (is_end_ != other.is_end_) return false;
95
96 return tree_ == other.tree_ && current_id_ == other.current_id_ && start_id_ == other.start_id_ && end_id_ == other.end_id_;
97}
98
99bool MapTreeRangeIterator::operator!=(const MapTreeRangeIterator& other) const { return !(*this == other); }
100
101bool MapTreeRangeIterator::isValid() const { return !is_end_ && current_landmark_ != nullptr; }
102
103uint32_t MapTreeRangeIterator::getCurrentId() const { return current_id_; }
104
105bool MapTreeRangeIterator::isEmpty() const {
106 MapTreeRangeIterator temp = *this;
107 return temp.is_end_;
108}
109
110// Locates and positions iterator at first valid landmark within range bounds
111void MapTreeRangeIterator::findFirstInRange() {
112 if (!tree_ || !tree_->getRoot()) {
113 is_end_ = true;
114 return;
115 }
116
117 uint32_t search_start = start_id_;
118 if (start_boundary_ == BoundaryType::EXCLUSIVE) {
119 search_start++;
120 }
121
122 current_id_ = search_start;
123 while (current_id_ <= end_id_) {
124 try {
125 current_landmark_ = tree_->extractLandmarkNodePointer(current_id_);
126 if (current_landmark_ && isInRange(current_id_)) {
127 return;
128 }
129 } catch (...) {
130 }
131 current_id_++;
132 }
133
134 is_end_ = true;
135 current_landmark_ = nullptr;
136 current_id_ = 0;
137}
138
139// Locates and positions iterator at last valid landmark within range bounds
140void MapTreeRangeIterator::findLastInRange() {
141 if (!tree_ || !tree_->getRoot()) {
142 is_end_ = true;
143 return;
144 }
145
146 uint32_t search_end = end_id_;
147 if (end_boundary_ == BoundaryType::EXCLUSIVE) {
148 if (search_end == 0) {
149 is_end_ = true;
150 return;
151 }
152 search_end--;
153 }
154
155 current_id_ = search_end;
156 while (current_id_ >= start_id_ && current_id_ <= search_end) {
157 try {
158 current_landmark_ = tree_->extractLandmarkNodePointer(current_id_);
159 if (current_landmark_ && isInRange(current_id_)) {
160 return;
161 }
162 } catch (...) {
163 }
164
165 if (current_id_ == 0) break;
166 current_id_--;
167 }
168
169 is_end_ = true;
170 current_landmark_ = nullptr;
171 current_id_ = 0;
172}
173
174// Advances to next valid landmark within range, returns false if none found
175bool MapTreeRangeIterator::findNext() {
176 if (!tree_) return false;
177
178 uint32_t next_id = current_id_ + 1;
179 uint32_t max_search = end_id_;
180 if (end_boundary_ == BoundaryType::EXCLUSIVE) {
181 max_search--;
182 }
183
184 while (next_id <= max_search) {
185 try {
186 auto next_landmark = tree_->extractLandmarkNodePointer(next_id);
187 if (next_landmark && isInRange(next_id)) {
188 current_id_ = next_id;
189 current_landmark_ = next_landmark;
190 return true;
191 }
192 } catch (...) {
193 }
194 next_id++;
195 }
196
197 return false;
198}
199
200// Moves to previous valid landmark within range, returns false if none found
201bool MapTreeRangeIterator::findPrevious() {
202 if (!tree_) return false;
203
204 if (current_id_ == 0) return false;
205
206 uint32_t prev_id = current_id_ - 1;
207 uint32_t min_search = start_id_;
208 if (start_boundary_ == BoundaryType::EXCLUSIVE) {
209 min_search++;
210 }
211
212 while (prev_id >= min_search && prev_id < current_id_) {
213 try {
214 auto prev_landmark = tree_->extractLandmarkNodePointer(prev_id);
215 if (prev_landmark && isInRange(prev_id)) {
216 current_id_ = prev_id;
217 current_landmark_ = prev_landmark;
218 return true;
219 }
220 } catch (...) {
221 }
222
223 // Prevent underflow
224 if (prev_id == 0) break;
225 prev_id--;
226 }
227
228 return false;
229}
230
231bool MapTreeRangeIterator::isInRange(uint32_t landmark_id) const {
232 if (start_boundary_ == BoundaryType::INCLUSIVE) {
233 if (landmark_id < start_id_) return false;
234 } else {
235 if (landmark_id <= start_id_) return false;
236 }
237
238 if (end_boundary_ == BoundaryType::INCLUSIVE) {
239 if (landmark_id > end_id_) return false;
240 } else {
241 if (landmark_id >= end_id_) return false;
242 }
243
244 return true;
245}
246
247bool MapTreeRangeIterator::isLeafNode(const std::shared_ptr<MapNode>& node) const {
248 return node && node->key_value == 0 && node->landmark != nullptr;
249}
250
251MapTreeRange::MapTreeRange(const MapTree* tree, uint32_t start_id, uint32_t end_id, MapTreeRangeIterator::BoundaryType start_boundary,
252 MapTreeRangeIterator::BoundaryType end_boundary)
253 : tree_(tree), start_id_(start_id), end_id_(end_id), start_boundary_(start_boundary), end_boundary_(end_boundary) {}
254
255MapTreeRangeIterator MapTreeRange::begin() const { return MapTreeRangeIterator(tree_, start_id_, end_id_, start_boundary_, end_boundary_); }
256
257MapTreeRangeIterator MapTreeRange::end() const { return MapTreeRangeIterator(); }
258
259bool MapTreeRange::empty() const {
260 auto it = begin();
261 return it == end();
262}
263
264size_t MapTreeRange::size() const {
265 size_t count = 0;
266 for (auto it = begin(); it != end(); ++it) {
267 count++;
268 }
269 return count;
270}
271
272// Constructs optimized iterator using hint to select best traversal strategy
273OptimizedRangeIterator::OptimizedRangeIterator(const MapTree* tree, uint32_t start_id, uint32_t end_id, OptimizationHint hint)
274 : tree_(tree),
275 start_id_(start_id),
276 end_id_(end_id),
277 current_id_(start_id),
278 hint_(hint),
279 current_landmark_(nullptr),
280 is_end_(false),
281 cache_index_(0) {
282 if (!tree_) {
283 is_end_ = true;
284 return;
285 }
286
287 initializeOptimized();
288}
289
290OptimizedRangeIterator::OptimizedRangeIterator()
291 : tree_(nullptr),
292 start_id_(0),
293 end_id_(0),
294 current_id_(0),
295 hint_(OptimizationHint::SINGLE_ELEMENT),
296 current_landmark_(nullptr),
297 is_end_(true),
298 cache_index_(0) {}
299
300OptimizedRangeIterator& OptimizedRangeIterator::operator++() {
301 if (is_end_) return *this;
302
303 switch (hint_) {
304 case OptimizationHint::SINGLE_ELEMENT:
305 is_end_ = true;
306 current_landmark_ = nullptr;
307 break;
308
309 case OptimizationHint::SMALL_RANGE:
310 cache_index_++;
311 if (cache_index_ >= cached_ids_.size()) {
312 is_end_ = true;
313 current_landmark_ = nullptr;
314 } else {
315 current_id_ = cached_ids_[cache_index_];
316 current_landmark_ = tree_->extractLandmarkNodePointer(current_id_);
317 }
318 break;
319
320 case OptimizationHint::LARGE_RANGE:
321 case OptimizationHint::FULL_TRAVERSAL:
322 default:
323 do {
324 current_id_++;
325 if (current_id_ > end_id_) {
326 is_end_ = true;
327 current_landmark_ = nullptr;
328 break;
329 }
330 try {
331 current_landmark_ = tree_->extractLandmarkNodePointer(current_id_);
332 if (current_landmark_) break;
333 } catch (...) {
334 current_landmark_ = nullptr;
335 }
336 } while (true);
337 break;
338 }
339
340 return *this;
341}
342
343OptimizedRangeIterator::reference OptimizedRangeIterator::operator*() const {
344 if (is_end_ || !current_landmark_) {
345 static std::shared_ptr<Landmark> null_landmark = nullptr;
346 return null_landmark;
347 }
348 return const_cast<reference>(current_landmark_);
349}
350
351bool OptimizedRangeIterator::operator==(const OptimizedRangeIterator& other) const {
352 if (is_end_ && other.is_end_) return true;
353 if (is_end_ != other.is_end_) return false;
354 return current_id_ == other.current_id_;
355}
356
357bool OptimizedRangeIterator::operator!=(const OptimizedRangeIterator& other) const { return !(*this == other); }
358
359// Initializes iterator with optimization strategy based on hint type
360void OptimizedRangeIterator::initializeOptimized() {
361 switch (hint_) {
362 case OptimizationHint::SINGLE_ELEMENT:
363 try {
364 current_landmark_ = tree_->extractLandmarkNodePointer(start_id_);
365 if (!current_landmark_) {
366 is_end_ = true;
367 }
368 } catch (...) {
369 is_end_ = true;
370 }
371 break;
372
373 case OptimizationHint::SMALL_RANGE:
374 populateCache();
375 if (cached_ids_.empty()) {
376 is_end_ = true;
377 } else {
378 current_id_ = cached_ids_[0];
379 current_landmark_ = tree_->extractLandmarkNodePointer(current_id_);
380 }
381 break;
382
383 case OptimizationHint::LARGE_RANGE:
384 case OptimizationHint::FULL_TRAVERSAL:
385 default:
386 while (current_id_ <= end_id_) {
387 try {
388 current_landmark_ = tree_->extractLandmarkNodePointer(current_id_);
389 if (current_landmark_) break;
390 } catch (...) {
391 current_landmark_ = nullptr;
392 }
393 current_id_++;
394 }
395 if (current_id_ > end_id_) {
396 is_end_ = true;
397 current_landmark_ = nullptr;
398 }
399 break;
400 }
401}
402
403// Pre-loads valid landmark IDs into cache for faster small range iteration
404void OptimizedRangeIterator::populateCache() {
405 cached_ids_.clear();
406 cached_ids_.reserve(std::min(10u, end_id_ - start_id_ + 1));
407
408 for (uint32_t id = start_id_; id <= end_id_ && cached_ids_.size() < 10; ++id) {
409 try {
410 auto landmark = tree_->extractLandmarkNodePointer(id);
411 if (landmark) {
412 cached_ids_.push_back(id);
413 }
414 } catch (...) {
415 }
416 }
417}
418
419} // namespace fastslam
420