/
/
/
1/**
2 * @file range_iterator_demo.cpp
3 * @brief Demonstration of FastSLAM MapTree range iterator usage patterns
4 *
5 * This file demonstrates various usage patterns for the range iterators,
6 * showing how they integrate with the FastSLAM tree structure and
7 * provide efficient O(log n + k) range queries.
8 */
9
10#include "fast_slam/MapTree.hpp"
11#include "fast_slam/MapTreeIterator.hpp"
12#include <eigen3/Eigen/Core>
13#include <iostream>
14#include <chrono>
15#include <iomanip>
16
17namespace fastslam_demo {
18
19/**
20 * @brief Create a sample MapTree with landmarks for demonstration
21 */
22std::unique_ptr<fastslam::MapTree> createSampleTree() {
23 auto tree = std::make_unique<fastslam::MapTree>();
24
25 // Insert landmarks at various positions to demonstrate range queries
26 std::vector<uint32_t> landmark_ids = {
27 1, 5, 8, 12, 15, 18, 22, 25, 30, 35, 40, 45, 50, 60, 70, 80, 90, 100
28 };
29
30 for (uint32_t id : landmark_ids) {
31 auto landmark = std::make_shared<fastslam::Landmark>();
32 landmark->landmark_identifier = id;
33
34 // Position landmarks in a grid pattern for realistic scenario
35 double x = (id % 10) * 2.0;
36 double y = (id / 10) * 2.0;
37 landmark->landmark_pose = Eigen::Vector2d(x, y);
38 landmark->landmark_covariance = Eigen::Matrix2d::Identity() * 0.1;
39
40 tree->insertLandmark(landmark);
41 }
42
43 return tree;
44}
45
46/**
47 * @brief Demonstrate basic range iteration
48 */
49void demonstrateBasicRangeIteration(const fastslam::MapTree& tree) {
50 std::cout << "\n=== Basic Range Iteration Demo ===\n";
51
52 // Example 1: Simple range query
53 std::cout << "\n1. Landmarks in range [15, 35]:\n";
54 auto range = tree.range(15, 35);
55 for (const auto& landmark : range) {
56 if (landmark) {
57 std::cout << " ID: " << std::setw(3) << landmark->landmark_identifier
58 << ", Position: (" << std::setprecision(1) << std::fixed
59 << landmark->landmark_pose.x() << ", "
60 << landmark->landmark_pose.y() << ")\n";
61 }
62 }
63
64 // Example 2: Count landmarks in range
65 auto count = tree.countLandmarksInRange(20, 50);
66 std::cout << "\n2. Number of landmarks in range [20, 50]: " << count << "\n";
67
68 // Example 3: Check if range has any landmarks
69 bool has_landmarks = tree.hasLandmarkInRange(75, 85);
70 std::cout << "\n3. Range [75, 85] has landmarks: " << (has_landmarks ? "Yes" : "No") << "\n";
71}
72
73/**
74 * @brief Demonstrate boundary type variations
75 */
76void demonstrateBoundaryTypes(const fastslam::MapTree& tree) {
77 std::cout << "\n=== Boundary Type Variations Demo ===\n";
78
79 uint32_t start_id = 22, end_id = 40;
80
81 // Inclusive boundaries
82 std::cout << "\n1. Range [" << start_id << ", " << end_id << "] (inclusive):\n";
83 auto inclusive_range = tree.rangeInclusive(start_id, end_id);
84 std::cout << " IDs: ";
85 for (const auto& landmark : inclusive_range) {
86 if (landmark) {
87 std::cout << landmark->landmark_identifier << " ";
88 }
89 }
90 std::cout << "\n";
91
92 // Exclusive boundaries
93 std::cout << "\n2. Range (" << start_id << ", " << end_id << ") (exclusive):\n";
94 auto exclusive_range = tree.rangeExclusive(start_id, end_id);
95 std::cout << " IDs: ";
96 for (const auto& landmark : exclusive_range) {
97 if (landmark) {
98 std::cout << landmark->landmark_identifier << " ";
99 }
100 }
101 std::cout << "\n";
102
103 // Mixed boundaries
104 std::cout << "\n3. Range [" << start_id << ", " << end_id << ") (start inclusive, end exclusive):\n";
105 auto mixed_range = tree.rangeMixed(start_id, end_id, true, false);
106 std::cout << " IDs: ";
107 for (const auto& landmark : mixed_range) {
108 if (landmark) {
109 std::cout << landmark->landmark_identifier << " ";
110 }
111 }
112 std::cout << "\n";
113}
114
115/**
116 * @brief Demonstrate optimized iterator patterns
117 */
118void demonstrateOptimizedPatterns(const fastslam::MapTree& tree) {
119 std::cout << "\n=== Optimized Iterator Patterns Demo ===\n";
120
121 // Single element access
122 std::cout << "\n1. Single element access (ID: 25):\n";
123 auto single_it = tree.singleElement(25);
124 if (*single_it) {
125 std::cout << " Found landmark " << (*single_it)->landmark_identifier
126 << " at position (" << (*single_it)->landmark_pose.x()
127 << ", " << (*single_it)->landmark_pose.y() << ")\n";
128 }
129
130 // Small range optimization
131 std::cout << "\n2. Small range optimization [18, 30]:\n";
132 auto small_range_it = tree.smallRange(18, 30);
133 std::cout << " IDs found: ";
134 while (small_range_it != fastslam::OptimizedRangeIterator(nullptr, 0, 0,
135 fastslam::OptimizedRangeIterator::OptimizationHint::SMALL_RANGE)) {
136 if (*small_range_it) {
137 std::cout << (*small_range_it)->landmark_identifier << " ";
138 }
139 ++small_range_it;
140 }
141 std::cout << "\n";
142
143 // Full traversal
144 std::cout << "\n3. Full tree traversal (first 10 landmarks):\n";
145 auto full_it = tree.fullTraversal();
146 int count = 0;
147 std::cout << " IDs: ";
148 while (count < 10 && full_it != fastslam::OptimizedRangeIterator(nullptr, 0, 0,
149 fastslam::OptimizedRangeIterator::OptimizationHint::FULL_TRAVERSAL)) {
150 if (*full_it) {
151 std::cout << (*full_it)->landmark_identifier << " ";
152 count++;
153 }
154 ++full_it;
155 }
156 std::cout << "\n";
157}
158
159/**
160 * @brief Demonstrate STL algorithm integration
161 */
162void demonstrateSTLIntegration(const fastslam::MapTree& tree) {
163 std::cout << "\n=== STL Algorithm Integration Demo ===\n";
164
165 auto range = tree.range(20, 60);
166
167 // Use std::find_if to locate specific landmark
168 auto it = std::find_if(range.begin(), range.end(),
169 [](const std::shared_ptr<fastslam::Landmark>& landmark) {
170 return landmark && landmark->landmark_pose.x() > 4.0;
171 });
172
173 if (it != range.end() && *it) {
174 std::cout << "\n1. First landmark with x > 4.0:\n";
175 std::cout << " ID: " << (*it)->landmark_identifier
176 << ", Position: (" << (*it)->landmark_pose.x()
177 << ", " << (*it)->landmark_pose.y() << ")\n";
178 }
179
180 // Use std::count_if to count landmarks matching criteria
181 auto high_y_count = std::count_if(range.begin(), range.end(),
182 [](const std::shared_ptr<fastslam::Landmark>& landmark) {
183 return landmark && landmark->landmark_pose.y() >= 4.0;
184 });
185
186 std::cout << "\n2. Landmarks with y >= 4.0 in range [20, 60]: " << high_y_count << "\n";
187
188 // Use std::distance to measure range size
189 auto range_size = std::distance(range.begin(), range.end());
190 std::cout << "\n3. Total landmarks in range [20, 60]: " << range_size << "\n";
191}
192
193/**
194 * @brief Demonstrate bidirectional iteration
195 */
196void demonstrateBidirectionalIteration(const fastslam::MapTree& tree) {
197 std::cout << "\n=== Bidirectional Iteration Demo ===\n";
198
199 auto range = tree.range(25, 50);
200
201 // Forward iteration
202 std::cout << "\n1. Forward iteration:\n ";
203 for (auto it = range.begin(); it != range.end(); ++it) {
204 if (*it) {
205 std::cout << (*it)->landmark_identifier << " -> ";
206 }
207 }
208 std::cout << "END\n";
209
210 // Reverse iteration
211 std::cout << "\n2. Reverse iteration:\n ";
212 auto it = range.end();
213 std::cout << "END";
214 while (it != range.begin()) {
215 --it;
216 if (*it) {
217 std::cout << " <- " << (*it)->landmark_identifier;
218 }
219 }
220 std::cout << "\n";
221
222 // Mixed iteration pattern
223 std::cout << "\n3. Mixed iteration (forward then backward):\n";
224 auto forward_it = range.begin();
225 if (forward_it != range.end() && *forward_it) {
226 std::cout << " First: " << (*forward_it)->landmark_identifier << "\n";
227
228 ++forward_it;
229 if (forward_it != range.end() && *forward_it) {
230 std::cout << " Second: " << (*forward_it)->landmark_identifier << "\n";
231
232 --forward_it; // Go back to first
233 if (*forward_it) {
234 std::cout << " Back to first: " << (*forward_it)->landmark_identifier << "\n";
235 }
236 }
237 }
238}
239
240/**
241 * @brief Performance comparison between different access methods
242 */
243void performanceComparison(const fastslam::MapTree& tree) {
244 std::cout << "\n=== Performance Comparison Demo ===\n";
245
246 const uint32_t start_id = 20, end_id = 80;
247 const int iterations = 1000;
248
249 // Method 1: Range iterator
250 auto start_time = std::chrono::high_resolution_clock::now();
251 for (int i = 0; i < iterations; ++i) {
252 auto range = tree.range(start_id, end_id);
253 size_t count = 0;
254 for (const auto& landmark : range) {
255 if (landmark) count++;
256 }
257 }
258 auto end_time = std::chrono::high_resolution_clock::now();
259 auto range_duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
260
261 // Method 2: Individual extractions
262 start_time = std::chrono::high_resolution_clock::now();
263 for (int i = 0; i < iterations; ++i) {
264 size_t count = 0;
265 for (uint32_t id = start_id; id <= end_id; ++id) {
266 try {
267 auto landmark = tree.extractLandmarkNodePointer(id);
268 if (landmark) count++;
269 } catch (...) {
270 // Landmark not found
271 }
272 }
273 }
274 end_time = std::chrono::high_resolution_clock::now();
275 auto individual_duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
276
277 std::cout << "\nPerformance comparison (" << iterations << " iterations):\n";
278 std::cout << " Range Iterator: " << range_duration.count() << " µs\n";
279 std::cout << " Individual Extraction: " << individual_duration.count() << " µs\n";
280 std::cout << " Speedup: " << std::setprecision(2)
281 << (double)individual_duration.count() / range_duration.count() << "x\n";
282}
283
284/**
285 * @brief Demonstrate copy-on-write preservation
286 */
287void demonstrateCopyOnWrite(const fastslam::MapTree& original_tree) {
288 std::cout << "\n=== Copy-on-Write Preservation Demo ===\n";
289
290 // Create a copy
291 fastslam::MapTree tree_copy(original_tree);
292
293 // Show that both trees have the same range content
294 std::cout << "\n1. Range [30, 50] in original tree:\n ";
295 auto orig_range = original_tree.range(30, 50);
296 for (const auto& landmark : orig_range) {
297 if (landmark) {
298 std::cout << landmark->landmark_identifier << " ";
299 }
300 }
301
302 std::cout << "\n\n2. Range [30, 50] in copied tree:\n ";
303 auto copy_range = tree_copy.range(30, 50);
304 for (const auto& landmark : copy_range) {
305 if (landmark) {
306 std::cout << landmark->landmark_identifier << " ";
307 }
308 }
309
310 std::cout << "\n\n3. Both ranges should be identical (preserves shared state)\n";
311
312 // Verify that iterators don't modify shared state
313 auto orig_it = orig_range.begin();
314 auto copy_it = copy_range.begin();
315
316 if (*orig_it && *copy_it) {
317 std::cout << " Original first element ID: " << (*orig_it)->landmark_identifier << "\n";
318 std::cout << " Copy first element ID: " << (*copy_it)->landmark_identifier << "\n";
319 std::cout << " Memory addresses " << ((*orig_it).get() == (*copy_it).get() ? "same" : "different")
320 << " (should be same due to COW)\n";
321 }
322}
323
324} // namespace fastslam_demo
325
326/**
327 * @brief Main demonstration function
328 */
329int main() {
330 std::cout << "FastSLAM MapTree Range Iterator Demonstration\n";
331 std::cout << "============================================\n";
332
333 // Create sample tree
334 auto tree = fastslam_demo::createSampleTree();
335
336 std::cout << "\nCreated tree with " << tree->getNLandmarks()
337 << " landmarks across " << tree->getNLayers() << " layers\n";
338
339 // Run demonstrations
340 fastslam_demo::demonstrateBasicRangeIteration(*tree);
341 fastslam_demo::demonstrateBoundaryTypes(*tree);
342 fastslam_demo::demonstrateOptimizedPatterns(*tree);
343 fastslam_demo::demonstrateSTLIntegration(*tree);
344 fastslam_demo::demonstrateBidirectionalIteration(*tree);
345 fastslam_demo::performanceComparison(*tree);
346 fastslam_demo::demonstrateCopyOnWrite(*tree);
347
348 std::cout << "\n=== Demo Complete ===\n";
349 std::cout << "The range iterators provide efficient O(log n + k) access\n";
350 std::cout << "to landmarks in the FastSLAM tree while preserving all\n";
351 std::cout << "copy-on-write semantics and thread safety properties.\n";
352
353 return 0;
354}
355
356/**
357 * Usage examples for different scenarios:
358 *
359 * // Nearby landmarks for sensor processing
360 * auto nearby = tree.range(current_id - 5, current_id + 5);
361 * for (const auto& landmark : nearby) {
362 * processSensorMeasurement(landmark);
363 * }
364 *
365 * // Batch processing with boundaries
366 * auto batch = tree.rangeExclusive(start_batch, end_batch);
367 * std::vector<LandmarkUpdate> updates;
368 * for (const auto& landmark : batch) {
369 * updates.push_back(processLandmark(landmark));
370 * }
371 *
372 * // Performance-critical single access
373 * auto single = tree.singleElement(target_id);
374 * if (*single) {
375 * return processCriticalLandmark(*single);
376 * }
377 *
378 * // Optimized small range with caching
379 * auto small = tree.smallRange(id1, id2);
380 * while (small != tree.smallRange(0, 0)) { // End condition
381 * processOptimized(*small);
382 * ++small;
383 * }
384 *
385 * // Full tree analysis
386 * auto full = tree.fullTraversal();
387 * AnalysisStats stats;
388 * while (full != tree.fullTraversal().end()) {
389 * stats.update(*full);
390 * ++full;
391 * }
392 */