fast_slam

1#include <gtest/gtest.h>
2#include "fast_slam/MapTree.hpp"
3#include "fast_slam/MapTreeIterator.hpp"
4#include <eigen3/Eigen/Core>
5#include <algorithm>
6#include <vector>
7#include <set>
8
9/**
10 * @brief Comprehensive tests for MapTree range iterators
11 * 
12 * These tests validate the range iterator functionality for the FastSLAM
13 * tree structure, ensuring O(log n + k) complexity and proper handling
14 * of all edge cases.
15 */
16
17class MapTreeIteratorTester : public ::testing::Test {
18protected:
19    std::unique_ptr<fastslam::MapTree> map_tree = std::make_unique<fastslam::MapTree>();
20    std::vector<uint32_t> inserted_ids;
21
22    virtual void SetUp() {
23        // Insert landmarks with gaps to test edge cases
24        std::vector<uint32_t> test_ids = {1, 3, 5, 7, 8, 10, 12, 15, 20, 25};
25        
26        for (uint32_t id : test_ids) {
27            std::shared_ptr<fastslam::Landmark> new_lm = std::make_shared<fastslam::Landmark>();
28            new_lm->landmark_identifier = id;
29            Eigen::Vector2d pose;
30            pose << 1.0 * id, 2.0 * id;
31            Eigen::Matrix2d cov = Eigen::Matrix2d::Identity();
32            new_lm->landmark_pose = pose;
33            new_lm->landmark_covariance = cov;
34            map_tree->insertLandmark(new_lm);
35            inserted_ids.push_back(id);
36        }
37        
38        std::sort(inserted_ids.begin(), inserted_ids.end());
39    }
40
41    // Helper function to collect IDs from iterator range
42    template<typename Iterator>
43    std::vector<uint32_t> collectIds(Iterator begin, Iterator end) {
44        std::vector<uint32_t> result;
45        for (auto it = begin; it != end; ++it) {
46            if (*it) {
47                result.push_back((*it)->landmark_identifier);
48            }
49        }
50        return result;
51    }
52};
53
54// Basic Range Iterator Tests
55
56TEST_F(MapTreeIteratorTester, BasicRangeIteration) {
57    // Test normal range [5, 15] inclusive
58    auto range = map_tree->range(5, 15);
59    auto collected_ids = collectIds(range.begin(), range.end());
60    
61    std::vector<uint32_t> expected = {5, 7, 8, 10, 12, 15};
62    EXPECT_EQ(collected_ids, expected);
63}
64
65TEST_F(MapTreeIteratorTester, EmptyRange) {
66    // Test empty range (no landmarks in range)
67    auto range = map_tree->range(2, 4);
68    auto collected_ids = collectIds(range.begin(), range.end());
69    
70    EXPECT_TRUE(collected_ids.empty());
71    EXPECT_TRUE(range.empty());
72}
73
74TEST_F(MapTreeIteratorTester, SingleElementRange) {
75    // Test single element range
76    auto range = map_tree->range(8, 8);
77    auto collected_ids = collectIds(range.begin(), range.end());
78    
79    std::vector<uint32_t> expected = {8};
80    EXPECT_EQ(collected_ids, expected);
81}
82
83TEST_F(MapTreeIteratorTester, FullRange) {
84    // Test full range
85    auto range = map_tree->range(1, 25);
86    auto collected_ids = collectIds(range.begin(), range.end());
87    
88    EXPECT_EQ(collected_ids, inserted_ids);
89}
90
91TEST_F(MapTreeIteratorTester, BoundaryEdgeCases) {
92    // Test boundaries that don't exist
93    auto range1 = map_tree->range(0, 30);
94    auto collected_ids1 = collectIds(range1.begin(), range1.end());
95    EXPECT_EQ(collected_ids1, inserted_ids);
96
97    // Test range starting at non-existent landmark
98    auto range2 = map_tree->range(2, 8);
99    auto collected_ids2 = collectIds(range2.begin(), range2.end());
100    std::vector<uint32_t> expected2 = {3, 5, 7, 8};
101    EXPECT_EQ(collected_ids2, expected2);
102}
103
104// Boundary Type Tests
105
106TEST_F(MapTreeIteratorTester, InclusiveBoundaries) {
107    auto range = map_tree->rangeInclusive(7, 12);
108    auto collected_ids = collectIds(range.begin(), range.end());
109    
110    std::vector<uint32_t> expected = {7, 8, 10, 12};
111    EXPECT_EQ(collected_ids, expected);
112}
113
114TEST_F(MapTreeIteratorTester, ExclusiveBoundaries) {
115    auto range = map_tree->rangeExclusive(7, 12);
116    auto collected_ids = collectIds(range.begin(), range.end());
117    
118    std::vector<uint32_t> expected = {8, 10};
119    EXPECT_EQ(collected_ids, expected);
120}
121
122TEST_F(MapTreeIteratorTester, MixedBoundaries) {
123    // Start inclusive, end exclusive
124    auto range1 = map_tree->rangeMixed(7, 12, true, false);
125    auto collected_ids1 = collectIds(range1.begin(), range1.end());
126    std::vector<uint32_t> expected1 = {7, 8, 10};
127    EXPECT_EQ(collected_ids1, expected1);
128
129    // Start exclusive, end inclusive
130    auto range2 = map_tree->rangeMixed(7, 12, false, true);
131    auto collected_ids2 = collectIds(range2.begin(), range2.end());
132    std::vector<uint32_t> expected2 = {8, 10, 12};
133    EXPECT_EQ(collected_ids2, expected2);
134}
135
136// Iterator Operations Tests
137
138TEST_F(MapTreeIteratorTester, ForwardIteration) {
139    auto range = map_tree->range(5, 15);
140    auto it = range.begin();
141    
142    EXPECT_EQ((*it)->landmark_identifier, 5);
143    
144    ++it;
145    EXPECT_EQ((*it)->landmark_identifier, 7);
146    
147    it++;
148    EXPECT_EQ((*it)->landmark_identifier, 8);
149    
150    ++it;
151    EXPECT_EQ((*it)->landmark_identifier, 10);
152}
153
154TEST_F(MapTreeIteratorTester, BackwardIteration) {
155    auto range = map_tree->range(5, 15);
156    auto it = range.end();
157    
158    --it;
159    EXPECT_EQ((*it)->landmark_identifier, 15);
160    
161    it--;
162    EXPECT_EQ((*it)->landmark_identifier, 12);
163    
164    --it;
165    EXPECT_EQ((*it)->landmark_identifier, 10);
166}
167
168TEST_F(MapTreeIteratorTester, IteratorComparison) {
169    auto range = map_tree->range(5, 15);
170    auto it1 = range.begin();
171    auto it2 = range.begin();
172    auto end_it = range.end();
173    
174    EXPECT_TRUE(it1 == it2);
175    EXPECT_FALSE(it1 != it2);
176    EXPECT_FALSE(it1 == end_it);
177    EXPECT_TRUE(it1 != end_it);
178    
179    ++it1;
180    EXPECT_FALSE(it1 == it2);
181    EXPECT_TRUE(it1 != it2);
182}
183
184// STL Algorithm Compatibility Tests
185
186TEST_F(MapTreeIteratorTester, STLAlgorithmCompatibility) {
187    auto range = map_tree->range(5, 15);
188    
189    // Test std::distance
190    auto distance = std::distance(range.begin(), range.end());
191    EXPECT_EQ(distance, 6); // 5, 7, 8, 10, 12, 15
192    
193    // Test std::find_if
194    auto it = std::find_if(range.begin(), range.end(), 
195        [](const std::shared_ptr<fastslam::Landmark>& landmark) {
196            return landmark->landmark_identifier == 10;
197        });
198    
199    EXPECT_NE(it, range.end());
200    EXPECT_EQ((*it)->landmark_identifier, 10);
201}
202
203// Optimized Iterator Tests
204
205TEST_F(MapTreeIteratorTester, SingleElementOptimization) {
206    auto it = map_tree->singleElement(8);
207    
208    EXPECT_EQ((*it)->landmark_identifier, 8);
209    
210    ++it;
211    // Should be at end after incrementing single element
212    EXPECT_EQ(it, map_tree->optimizedEnd());
213}
214
215TEST_F(MapTreeIteratorTester, SmallRangeOptimization) {
216    auto it = map_tree->smallRange(7, 12);
217    
218    std::vector<uint32_t> collected_ids;
219    while (it != map_tree->optimizedEnd()) {
220        if (*it) {
221            collected_ids.push_back((*it)->landmark_identifier);
222        }
223        ++it;
224    }
225    
226    std::vector<uint32_t> expected = {7, 8, 10, 12};
227    EXPECT_EQ(collected_ids, expected);
228}
229
230// Utility Method Tests
231
232TEST_F(MapTreeIteratorTester, GetLandmarksInRange) {
233    auto landmarks = map_tree->getLandmarksInRange(7, 12);
234    
235    EXPECT_EQ(landmarks.size(), 4);
236    EXPECT_EQ(landmarks[0]->landmark_identifier, 7);
237    EXPECT_EQ(landmarks[1]->landmark_identifier, 8);
238    EXPECT_EQ(landmarks[2]->landmark_identifier, 10);
239    EXPECT_EQ(landmarks[3]->landmark_identifier, 12);
240}
241
242TEST_F(MapTreeIteratorTester, CountLandmarksInRange) {
243    auto count1 = map_tree->countLandmarksInRange(5, 15);
244    EXPECT_EQ(count1, 6);
245    
246    auto count2 = map_tree->countLandmarksInRange(2, 4);
247    EXPECT_EQ(count2, 0);
248    
249    auto count3 = map_tree->countLandmarksInRange(8, 8);
250    EXPECT_EQ(count3, 1);
251}
252
253TEST_F(MapTreeIteratorTester, HasLandmarkInRange) {
254    EXPECT_TRUE(map_tree->hasLandmarkInRange(7, 12));
255    EXPECT_FALSE(map_tree->hasLandmarkInRange(2, 4));
256    EXPECT_TRUE(map_tree->hasLandmarkInRange(8, 8));
257    EXPECT_FALSE(map_tree->hasLandmarkInRange(9, 9));
258}
259
260// Performance and Edge Case Tests
261
262TEST_F(MapTreeIteratorTester, LargeRangePerformance) {
263    // Add more landmarks for performance testing
264    std::unique_ptr<fastslam::MapTree> large_tree = std::make_unique<fastslam::MapTree>();
265    
266    // Insert 1000 landmarks with some gaps
267    for (uint32_t i = 1; i <= 1000; i += 2) { // Only odd numbers
268        std::shared_ptr<fastslam::Landmark> new_lm = std::make_shared<fastslam::Landmark>();
269        new_lm->landmark_identifier = i;
270        Eigen::Vector2d pose;
271        pose << 1.0 * i, 2.0 * i;
272        new_lm->landmark_covariance = Eigen::Matrix2d::Identity();
273        new_lm->landmark_pose = pose;
274        large_tree->insertLandmark(new_lm);
275    }
276    
277    // Test range iteration performance
278    auto range = large_tree->range(100, 200);
279    auto start_time = std::chrono::high_resolution_clock::now();
280    
281    size_t count = 0;
282    for (auto& landmark : range) {
283        if (landmark) count++;
284    }
285    
286    auto end_time = std::chrono::high_resolution_clock::now();
287    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
288    
289    EXPECT_EQ(count, 50); // 50 odd numbers from 101 to 199
290    
291    // Performance should be reasonable (adjust threshold as needed)
292    EXPECT_LT(duration.count(), 10000); // Less than 10ms
293}
294
295TEST_F(MapTreeIteratorTester, ConcurrentReadAccess) {
296    // Test thread safety for read operations
297    // This is a basic test - full thread safety testing would require more complex scenarios
298    auto range = map_tree->range(1, 25);
299    
300    std::vector<std::vector<uint32_t>> results(4);
301    
302    // Simulate concurrent access (though actual concurrency would need std::thread)
303    for (int i = 0; i < 4; ++i) {
304        auto local_range = map_tree->range(1, 25);
305        results[i] = collectIds(local_range.begin(), local_range.end());
306    }
307    
308    // All results should be identical
309    for (int i = 1; i < 4; ++i) {
310        EXPECT_EQ(results[0], results[i]);
311    }
312}
313
314TEST_F(MapTreeIteratorTester, InvalidRangeHandling) {
315    // Test invalid ranges
316    auto range1 = map_tree->range(15, 5); // start > end
317    EXPECT_TRUE(range1.empty());
318    
319    // Test ranges on empty tree
320    std::unique_ptr<fastslam::MapTree> empty_tree = std::make_unique<fastslam::MapTree>();
321    auto range2 = empty_tree->range(1, 10);
322    EXPECT_TRUE(range2.empty());
323}
324
325// Copy-on-Write Preservation Tests
326
327TEST_F(MapTreeIteratorTester, CopyOnWritePreservation) {
328    // Create a copy of the tree
329    std::unique_ptr<fastslam::MapTree> tree_copy = std::make_unique<fastslam::MapTree>(*map_tree);
330    
331    // Test that both trees return the same ranges
332    auto range1 = map_tree->range(5, 15);
333    auto range2 = tree_copy->range(5, 15);
334    
335    auto ids1 = collectIds(range1.begin(), range1.end());
336    auto ids2 = collectIds(range2.begin(), range2.end());
337    
338    EXPECT_EQ(ids1, ids2);
339    
340    // Verify that iterators are read-only and don't modify shared state
341    auto it1 = range1.begin();
342    auto it2 = range2.begin();
343    
344    // Both should point to same landmark data initially
345    EXPECT_EQ((*it1)->landmark_identifier, (*it2)->landmark_identifier);
346    
347    ++it1;
348    ++it2;
349    
350    // Should still be consistent
351    EXPECT_EQ((*it1)->landmark_identifier, (*it2)->landmark_identifier);
352}