/
/
/
1#include "fast_slam/MapTree.hpp"
2#include "fast_slam/MapTreeIterator.hpp"
3
4namespace fastslam{
5
6unsigned int Landmark::global_landmark_counter;
7
8uint32_t MapNode::global_map_node_counter;
9int MapTree::map_tree_identifier_counter = 1;
10
11MapTree::MapTree():
12 map_tree_identifier_(map_tree_identifier_counter),
13 n_landmarks_(0),
14 n_layers_(0),
15 n_nodes_(0),
16 map_root_(nullptr)
17 {
18 map_tree_identifier_counter++;
19}
20
21MapTree::MapTree(const MapTree &map_to_copy){
22 map_tree_identifier_ = map_tree_identifier_counter;
23
24 map_tree_identifier_counter++;
25 map_root_ = map_to_copy.map_root_;
26
27 if (map_to_copy.map_root_ != nullptr){
28 map_to_copy.map_root_->referenced++;
29 }
30
31 n_landmarks_ = map_to_copy.n_landmarks_;
32 n_layers_ = map_to_copy.n_layers_;
33 n_nodes_ = map_to_copy.n_nodes_;
34}
35
36MapTree::~MapTree(){}
37
38void MapTree::removeReferenceToSubTree(std::shared_ptr<MapNode> &sub_tree_root){
39 sub_tree_root.reset();
40}
41
42void MapTree::insertLandmark(std::shared_ptr<Landmark> &new_landmark){
43 if (new_landmark->landmark_identifier == 1){
44 int need_n_layers = 1;
45 if (need_n_layers > n_layers_){
46 creatNewLayers(need_n_layers);
47 }
48 }
49 else{
50 auto c_tmp = (float) new_landmark->landmark_identifier;
51 int need_n_layers = (int)ceil(log2(c_tmp));
52
53 if (need_n_layers > n_layers_){
54 creatNewLayers(need_n_layers);
55 }
56 }
57
58 std::shared_ptr<MapNode> *curr = &map_root_;
59 size_t i2 = ((*curr)->key_value) / 2;
60
61
62 for(size_t i = n_layers_; i>1; i--){
63
64 if(new_landmark->landmark_identifier > (*curr)->key_value){
65 if((*curr)->right != nullptr)
66 curr = &((*curr)->right);
67 else{
68 (*curr)->right = std::make_shared<MapNode>();
69 (*curr)->right->key_value = (*curr)->key_value + i2;
70 (*curr)->right->right = nullptr;
71 (*curr)->right->left = nullptr;
72 (*curr)->right->landmark = nullptr;
73 (*curr)->right->referenced = 1;
74
75 curr = &((*curr)->right);
76 n_nodes_++;
77 }
78 }
79 else{
80 if((*curr)->left != nullptr)
81 curr = &((*curr)->left);
82 else{
83 (*curr)->left = std::make_shared<MapNode>();
84 (*curr)->left->key_value = (*curr)->key_value - i2;
85 (*curr)->left->right = nullptr;
86 (*curr)->left->left = nullptr;
87 (*curr)->left->landmark = nullptr;
88 (*curr)->left->referenced = 1;
89
90 curr = &((*curr)->left);
91 n_nodes_++;
92 }
93 }
94 i2 = i2 / 2;
95 }
96
97
98 std::shared_ptr<MapNode> new_leaf = std::make_shared<MapNode>();
99 new_leaf->key_value = 0;
100 new_leaf->left = nullptr;
101 new_leaf->right = nullptr;
102 new_leaf->referenced = 1;
103 new_leaf->landmark = new_landmark;
104
105 if(new_landmark->landmark_identifier > (*curr)->key_value)
106 (*curr)->right = std::move(new_leaf);
107 else
108 (*curr)->left = std::move(new_leaf);
109
110 n_nodes_++;
111 n_landmarks_++;
112}
113
114void MapTree::creatNewLayers(int needed_n_layers){
115 int missing_layers = needed_n_layers - n_layers_;
116
117 for(size_t i = 1; i<= missing_layers; i++){
118 std::shared_ptr<MapNode> newmap_root_node = std::make_shared<MapNode>();
119 newmap_root_node->key_value = static_cast<int>(pow(2,(n_layers_ + i) - 1));
120 newmap_root_node->right = nullptr;
121 newmap_root_node->landmark = nullptr;
122 newmap_root_node->referenced = 1;
123
124
125 newmap_root_node->left = std::move(map_root_);
126 n_nodes_++;
127
128 map_root_ = std::move(newmap_root_node);
129 }
130
131 n_layers_ = needed_n_layers;
132}
133
134
135std::shared_ptr<MapNode> MapTree::extractLeafNodePointer(const std::shared_ptr<MapNode> ¤t_ptr, uint32_t landmark_identifier) const {
136
137 if(current_ptr == nullptr){
138 std::cout << "got a nullptr someting went wrong! \n";
139 return nullptr;
140 }
141
142 if(current_ptr->key_value == 0)
143 return current_ptr;
144
145 if(landmark_identifier > current_ptr->key_value)
146 return extractLeafNodePointer(current_ptr->right, landmark_identifier);
147
148 else
149 return extractLeafNodePointer(current_ptr->left, landmark_identifier);
150
151
152 return nullptr;
153}
154
155void MapTree::correctLandmark(std::shared_ptr<Landmark> &new_landmark_data){
156 map_root_ = makeNewPath(new_landmark_data, map_root_);
157}
158
159std::shared_ptr<MapNode> MapTree::makeNewPath(std::shared_ptr<Landmark> &new_landmark_data, std::shared_ptr<MapNode> &starting_node){
160 if(starting_node->key_value > 0){
161 std::shared_ptr<MapNode> new_map_node = std::make_shared<MapNode>();
162 new_map_node->landmark = nullptr;
163 new_map_node->referenced = 1;
164 new_map_node->key_value = starting_node->key_value;
165
166 if(new_landmark_data->landmark_identifier > starting_node->key_value){
167 new_map_node->left = starting_node->left;
168 if(new_map_node->left != nullptr)
169 new_map_node->left->referenced++;
170
171 new_map_node->right = makeNewPath(new_landmark_data, starting_node->right);
172 }
173 else if(new_landmark_data->landmark_identifier <= starting_node->key_value){
174 new_map_node->right = starting_node->right;
175 if(new_map_node->right != nullptr)
176 new_map_node->right->referenced++;
177
178 new_map_node->left = makeNewPath(new_landmark_data, starting_node->left);
179 }
180 else{
181 std::cout << "error in makeNewPath";
182 }
183 return new_map_node;
184 }
185 else{
186 std::shared_ptr<MapNode> new_leaf_node = std::make_shared<MapNode>();
187 new_leaf_node->key_value = 0;
188 new_leaf_node->left = nullptr;
189 new_leaf_node->right = nullptr;
190 new_leaf_node->landmark = new_landmark_data;
191 new_leaf_node->referenced = 1;
192
193 return new_leaf_node;
194 }
195}
196
197
198void MapTree::printAllLandmarkPositions(std::ostream &out){
199
200 for(size_t i = 1 ; i<=n_landmarks_ ; i++){
201 auto leaf = extractLeafNodePointer(map_root_, i);
202
203 if (leaf != nullptr){
204 out << "l_" << i <<" pose: "<< leaf->landmark->landmark_pose.transpose() << " at address: " << std::hex << leaf << std::dec << std::endl;
205 }
206 else{
207 out <<"Error: NULL pointer!";
208 }
209 }
210
211}
212
213/* Creates range object with default inclusive boundaries for for-loop iteration */
214MapTreeRange MapTree::range(uint32_t start_id, uint32_t end_id) const {
215 return MapTreeRange(this, start_id, end_id);
216}
217
218/* Creates range object including both start and end landmark IDs */
219MapTreeRange MapTree::rangeInclusive(uint32_t start_id, uint32_t end_id) const {
220 return MapTreeRange(this, start_id, end_id,
221 MapTreeRangeIterator::BoundaryType::INCLUSIVE,
222 MapTreeRangeIterator::BoundaryType::INCLUSIVE);
223}
224
225/* Creates range object excluding both start and end landmark IDs */
226MapTreeRange MapTree::rangeExclusive(uint32_t start_id, uint32_t end_id) const {
227 return MapTreeRange(this, start_id, end_id,
228 MapTreeRangeIterator::BoundaryType::EXCLUSIVE,
229 MapTreeRangeIterator::BoundaryType::EXCLUSIVE);
230}
231
232/* Creates range object with custom boundary inclusion for each end */
233MapTreeRange MapTree::rangeMixed(uint32_t start_id, uint32_t end_id, bool start_inclusive, bool end_inclusive) const {
234 return MapTreeRange(this, start_id, end_id,
235 start_inclusive ? MapTreeRangeIterator::BoundaryType::INCLUSIVE : MapTreeRangeIterator::BoundaryType::EXCLUSIVE,
236 end_inclusive ? MapTreeRangeIterator::BoundaryType::INCLUSIVE : MapTreeRangeIterator::BoundaryType::EXCLUSIVE);
237}
238
239/* Creates optimized iterator for accessing exactly one landmark */
240OptimizedRangeIterator MapTree::singleElement(uint32_t landmark_id) const {
241 return OptimizedRangeIterator(this, landmark_id, landmark_id, OptimizedRangeIterator::OptimizationHint::SINGLE_ELEMENT);
242}
243
244/* Creates cached iterator optimized for small ranges with fast access */
245OptimizedRangeIterator MapTree::smallRange(uint32_t start_id, uint32_t end_id) const {
246 return OptimizedRangeIterator(this, start_id, end_id, OptimizedRangeIterator::OptimizationHint::SMALL_RANGE);
247}
248
249/* Creates end iterator for optimized range comparisons */
250OptimizedRangeIterator MapTree::optimizedEnd() const {
251 return OptimizedRangeIterator();
252}
253
254/* Returns vector of all valid landmarks within specified ID range */
255std::vector<std::shared_ptr<Landmark>> MapTree::getLandmarksInRange(uint32_t start_id, uint32_t end_id) const {
256 std::vector<std::shared_ptr<Landmark>> landmarks;
257 for (uint32_t id = start_id; id <= end_id; ++id) {
258 try {
259 auto landmark = extractLandmarkNodePointer(id);
260 if (landmark) {
261 landmarks.push_back(landmark);
262 }
263 } catch (...) {
264 }
265 }
266 return landmarks;
267}
268
269/* Counts number of valid landmarks within specified ID range */
270size_t MapTree::countLandmarksInRange(uint32_t start_id, uint32_t end_id) const {
271 size_t count = 0;
272 for (uint32_t id = start_id; id <= end_id; ++id) {
273 try {
274 auto landmark = extractLandmarkNodePointer(id);
275 if (landmark) {
276 count++;
277 }
278 } catch (...) {
279 }
280 }
281 return count;
282}
283
284/* Checks if any valid landmarks exist within specified ID range */
285bool MapTree::hasLandmarkInRange(uint32_t start_id, uint32_t end_id) const {
286 for (uint32_t id = start_id; id <= end_id; ++id) {
287 try {
288 auto landmark = extractLandmarkNodePointer(id);
289 if (landmark) {
290 return true;
291 }
292 } catch (...) {
293 }
294 }
295 return false;
296}
297
298
299} //namespace fastslam