/
/
/
1#include "fast_slam/MapTree.hpp"
2
3namespace fastslam{
4
5// can be used to check if number of Landmarks does not grow without bound
6unsigned int Landmark::global_landmark_counter;
7
8// can be used to check if number of MapNodes does not grow without bound
9unsigned int MapNode::global_map_node_counter;
10int MapTree::map_tree_identifier_counter = 1;
11
12MapTree::MapTree():
13 map_tree_identifier_(map_tree_identifier_counter),
14 n_landmarks_(0),
15 n_layers_(0),
16 n_nodes_(0),
17 map_root_(nullptr)
18 {
19 map_tree_identifier_counter++;
20}
21
22MapTree::MapTree(const MapTree &map_to_copy){
23 map_tree_identifier_ = map_tree_identifier_counter;
24
25 map_tree_identifier_counter++;
26 map_root_ = map_to_copy.map_root_;
27
28 // we add new reference for a MapNode and have to increment its reference counter
29 if (map_to_copy.map_root_ != nullptr){
30 map_to_copy.map_root_->referenced++;
31 }
32
33 n_landmarks_ = map_to_copy.n_landmarks_;
34 n_layers_ = map_to_copy.n_layers_;
35 n_nodes_ = map_to_copy.n_nodes_;
36}
37
38MapTree::~MapTree(){}
39
40void MapTree::removeReferenceToSubTree(std::shared_ptr<MapNode> &sub_tree_root){
41 sub_tree_root.reset();
42}
43
44void MapTree::insertLandmark(std::shared_ptr<Landmark> &new_landmark){
45 // first landmark
46 if (new_landmark->landmark_identifier == 1){ // handle special case
47 int need_n_layers = 1;
48 if (need_n_layers > n_layers_){
49 creatNewLayers(need_n_layers);
50 }
51 }
52 else{
53 auto c_tmp = (float) new_landmark->landmark_identifier;
54 int need_n_layers = (int)ceil(log2(c_tmp));
55
56 if (need_n_layers > n_layers_){
57 creatNewLayers(need_n_layers);
58 }
59 }
60
61 // get pointer to make unique pointer iterable
62 std::shared_ptr<MapNode> *curr = &map_root_;
63 size_t i2 = ((*curr)->key_value) / 2;
64
65 /*
66 We are filling the tree in the following structure:
67 O(2) Level 2
68 / \
69 O(1) O(3) Level 1
70 / \ / \
71 O(0) O(0) O(0) O(0)
72 LM(1) LM(2) LM(3) LM(4)
73 */
74
75 for(size_t i = n_layers_; i>1; i--){
76
77 // if identifir is greater than current node go right
78 if(new_landmark->landmark_identifier > (*curr)->key_value){
79 // if the right node is not null use right as curr
80 if((*curr)->right != nullptr)
81 curr = &((*curr)->right);
82
83 // else create a new node
84 else{
85 (*curr)->right = std::make_shared<MapNode>();
86 (*curr)->right->key_value = (*curr)->key_value + i2;
87 (*curr)->right->right = nullptr;
88 (*curr)->right->left = nullptr;
89 (*curr)->right->landmark = nullptr;
90 (*curr)->right->referenced = 1;
91
92 curr = &((*curr)->right);
93 n_nodes_++;
94 }
95 }
96 // else go left
97 else{
98 // same here. If not null go left
99 if((*curr)->left != nullptr)
100 curr = &((*curr)->left);
101 // same here. If null, create new node
102 else{
103 (*curr)->left = std::make_shared<MapNode>();
104 (*curr)->left->key_value = (*curr)->key_value - i2;
105 (*curr)->left->right = nullptr;
106 (*curr)->left->left = nullptr;
107 (*curr)->left->landmark = nullptr;
108 (*curr)->left->referenced = 1;
109
110 curr = &((*curr)->left);
111 n_nodes_++;
112 }
113 }
114 i2 = i2 / 2;
115 }
116
117 // Now we are at the bottom, ready to add a new leaf
118
119 std::shared_ptr<MapNode> new_leaf = std::make_shared<MapNode>();
120 new_leaf->key_value = 0;
121 new_leaf->left = nullptr;
122 new_leaf->right = nullptr;
123 new_leaf->referenced = 1;
124 new_leaf->landmark = new_landmark;
125
126 // again, if landmark is greater than current key value, go right
127 if(new_landmark->landmark_identifier > (*curr)->key_value)
128 (*curr)->right = std::move(new_leaf);
129 else
130 (*curr)->left = std::move(new_leaf);
131
132 n_nodes_++;
133 n_landmarks_++;
134}
135
136void MapTree::creatNewLayers(int needed_n_layers){
137 int missing_layers = needed_n_layers - n_layers_;
138
139 for(size_t i = 1; i<= missing_layers; i++){
140 std::shared_ptr<MapNode> newmap_root_node = std::make_shared<MapNode>();
141 newmap_root_node->key_value = static_cast<int>(pow(2,(n_layers_ + i) - 1));
142 newmap_root_node->right = nullptr;
143 newmap_root_node->landmark = nullptr;
144 newmap_root_node->referenced = 1;
145
146 // give new node ownership of old root
147 newmap_root_node->left = std::move(map_root_);
148 n_nodes_++;
149
150 // move new node to root
151 map_root_ = std::move(newmap_root_node);
152 }
153
154 n_layers_ = needed_n_layers;
155}
156
157
158std::shared_ptr<MapNode> MapTree::extractLeafNodePointer(std::shared_ptr<MapNode> ¤t_ptr, uint32_t landmark_identifier){
159
160 if(current_ptr == nullptr){
161 std::cout << "got a nullptr someting went wrong! \n";
162 return nullptr;
163 }
164
165 if(current_ptr->key_value == 0)
166 return current_ptr;
167
168 // // go right
169 if(landmark_identifier > current_ptr->key_value)
170 return extractLeafNodePointer(current_ptr->right, landmark_identifier);
171
172 // // go left
173 else
174 return extractLeafNodePointer(current_ptr->left, landmark_identifier);
175
176
177 return nullptr;
178}
179
180void MapTree::correctLandmark(std::shared_ptr<Landmark> &new_landmark_data){
181 // reassignment should release the original pointer after assignment
182 map_root_ = makeNewPath(new_landmark_data, map_root_);
183}
184
185std::shared_ptr<MapNode> MapTree::makeNewPath(std::shared_ptr<Landmark> &new_landmark_data, std::shared_ptr<MapNode> &starting_node){
186 if(starting_node->key_value > 0){
187 std::shared_ptr<MapNode> new_map_node = std::make_shared<MapNode>();
188 new_map_node->landmark = nullptr;
189 new_map_node->referenced = 1;
190 new_map_node->key_value = starting_node->key_value;
191
192 if(new_landmark_data->landmark_identifier > starting_node->key_value){
193 // link entire left branch to original subtree since landmark is on the right
194 new_map_node->left = starting_node->left;
195 if(new_map_node->left != nullptr)
196 new_map_node->left->referenced++;
197
198 // now traverse until leaf nodes
199 new_map_node->right = makeNewPath(new_landmark_data, starting_node->right);
200 }
201 else if(new_landmark_data->landmark_identifier <= starting_node->key_value){
202 // link entire right branch to original subtree since landmark is on the left
203 new_map_node->right = starting_node->right;
204 if(new_map_node->right != nullptr)
205 new_map_node->right->referenced++;
206
207 // now traverse until leaf nodes
208 new_map_node->left = makeNewPath(new_landmark_data, starting_node->left);
209 }
210 else{
211 std::cout << "error in makeNewPath";
212 }
213 return new_map_node;
214 }
215 // now we reached the leaf nodes
216 else{
217 std::shared_ptr<MapNode> new_leaf_node = std::make_shared<MapNode>();
218 new_leaf_node->key_value = 0;
219 new_leaf_node->left = nullptr;
220 new_leaf_node->right = nullptr;
221 new_leaf_node->landmark = new_landmark_data;
222 new_leaf_node->referenced = 1;
223
224 return new_leaf_node;
225 }
226}
227
228
229void MapTree::printAllLandmarkPositions(std::ostream &out){
230
231 for(size_t i = 1 ; i<=n_landmarks_ ; i++){
232 auto leaf = extractLeafNodePointer(map_root_, i);
233
234 if (leaf != nullptr){
235 out << "l_" << i <<" pose: "<< leaf->landmark->landmark_pose.transpose() << " at address: " << std::hex << leaf << std::dec << std::endl;
236 }
237 else{
238 out <<"Error: NULL pointer!";
239 }
240 }
241
242}
243
244
245} //namespace fastslam
246