/
/
/
1#include <gtest/gtest.h>
2
3#include "fast_slam/MapTree.hpp"
4#include <eigen3/Eigen/Core>
5
6/*
7Balanced binary tree of map with 10 landmarks should look like this:
8
9 8
10 / \
11 4 12
12 / \ /
13 2 6 10
14 / \ / \ /
15 1 3 5 7 9
16 / \ / \ / \ / \ / \
17 1 2 3 4 5 6 7 8 9 10
18*/
19
20class MapTreeTester : public ::testing::Test {
21protected:
22 std::unique_ptr<fastslam::MapTree> map_tree = std::make_unique<fastslam::MapTree>();
23
24 virtual void SetUp(){
25 for(size_t i=1;i<=10; i++){
26 std::shared_ptr<fastslam::Landmark> new_lm = std::make_shared<fastslam::Landmark>();
27 new_lm->landmark_identifier = i;
28 Eigen::Vector2d pose;
29 pose << 1.0 * i, 2.0 * i;
30 Eigen::Matrix2d cov = Eigen::Matrix2d::Identity();
31 new_lm->landmark_pose = pose;
32 new_lm->landmark_covariance = cov;
33 map_tree->insertLandmark(new_lm);
34 }
35 }
36};
37
38TEST_F(MapTreeTester, nLandmarks_test){
39 auto n_landmarks = map_tree->getNLandmarks();
40 EXPECT_EQ(n_landmarks, 10);
41}
42
43TEST_F(MapTreeTester, nLayers_test){
44 auto n_layers = map_tree->getNLayers();
45 EXPECT_EQ(n_layers, 4);
46}
47
48TEST_F(MapTreeTester, nNodes_test){
49 auto n_nodes = map_tree->getNnodes();
50 EXPECT_EQ(n_nodes, 21);
51}
52
53TEST_F(MapTreeTester, extractLandmark_test){
54
55 // test first
56 auto lm_back = map_tree->extractLandmarkNodePointer(1);
57
58 EXPECT_DOUBLE_EQ(lm_back->landmark_pose(0), 1.0 * 1);
59 EXPECT_DOUBLE_EQ(lm_back->landmark_pose(1), 2.0 * 1);
60
61 // test last
62 lm_back = map_tree->extractLandmarkNodePointer(10);
63
64 EXPECT_DOUBLE_EQ(lm_back->landmark_pose(0), 1.0 * 10);
65 EXPECT_DOUBLE_EQ(lm_back->landmark_pose(1), 2.0 * 10);
66
67 // test middle odd
68 lm_back = map_tree->extractLandmarkNodePointer(5);
69
70 EXPECT_DOUBLE_EQ(lm_back->landmark_pose(0), 1.0 * 5);
71 EXPECT_DOUBLE_EQ(lm_back->landmark_pose(1), 2.0 * 5);
72
73 // test middle even
74 lm_back = map_tree->extractLandmarkNodePointer(8);
75
76 EXPECT_DOUBLE_EQ(lm_back->landmark_pose(0), 1.0 * 8);
77 EXPECT_DOUBLE_EQ(lm_back->landmark_pose(1), 2.0 * 8);
78}
79
80TEST_F(MapTreeTester, copy_test){
81 std::unique_ptr<fastslam::MapTree> map_tree_copy = std::make_unique<fastslam::MapTree>(*map_tree);
82
83 // prints contents and addresses of leaf nodes
84 // since we want the resource to be shared a copy here means same addresses for all leafs
85 std::stringstream out, out_copy;
86 out << *map_tree;
87 out_copy << *map_tree_copy;
88
89 EXPECT_EQ(out.str(), out_copy.str());
90}
91
92TEST_F(MapTreeTester, copyAndChange_test){
93 std::unique_ptr<fastslam::MapTree> map_tree_copy = std::make_unique<fastslam::MapTree>(*map_tree);
94
95 // correct the 10th landmark
96 std::shared_ptr<fastslam::Landmark> new_lm = std::make_shared<fastslam::Landmark>();
97 new_lm->landmark_identifier = 10;
98 Eigen::Vector2d pose;
99 pose << 1.0 , 2.0;
100 Eigen::Matrix2d cov = Eigen::Matrix2d::Identity();
101 new_lm->landmark_pose = pose;
102 new_lm->landmark_covariance = cov;
103
104 map_tree_copy->correctLandmark(new_lm);
105
106
107 std::stringstream out, out_copy;
108 out << *map_tree;
109 out_copy << *map_tree_copy;
110
111
112 for(size_t i=1; i<=10; i++){
113 std::string line, line_copy;
114 std::getline(out, line);
115 std::getline(out_copy, line_copy);
116 if(i == 10){
117 EXPECT_FALSE(line == line_copy);
118 }
119 else{
120 EXPECT_TRUE(line == line_copy);
121 }
122 }
123}