| 6 | } |
| 7 | |
| 8 | void MapManager::updateMap(std::vector<Cylinder>& obs_tms, const std::vector<int>& matches){ |
| 9 | // for(auto i = 0; i < matches.size(); i++){ |
| 10 | size_t i = 0; |
| 11 | for(auto const& tree : obs_tms){ |
| 12 | PointT pt; |
| 13 | pt.x = tree.model.root[0]; pt.y = tree.model.root[1]; pt.z = tree.model.root[2]; |
| 14 | if(matches[i] == -1){ |
| 15 | landmarks_->push_back(pt); |
| 16 | treeModels_.push_back(tree); |
| 17 | treeHits_.push_back(1); |
| 18 | } else { |
| 19 | // transform from observation to map index |
| 20 | int matchIdx = matchesMap[matches[i]]; |
| 21 | landmarks_->points[matchIdx] = pt; |
| 22 | treeModels_[matchIdx] = tree; |
| 23 | treeHits_[matchIdx] += 1; |
| 24 | } |
| 25 | i++; |
| 26 | } |
| 27 | matchesMap.clear(); |
| 28 | } |
| 29 | |
| 30 | std::vector<Cylinder> MapManager::getMap() |
| 31 | { |