| 39 | } |
| 40 | |
| 41 | void MapManager::getSubmap(const SE3& pose, std::vector<Cylinder>& submap){ |
| 42 | if(landmarks_->size() == 0) return; |
| 43 | |
| 44 | pcl::KdTreeFLANN<PointT> kdtree; |
| 45 | kdtree.setInputCloud(landmarks_); |
| 46 | std::vector<int> pointIdxKNNSearch; |
| 47 | std::vector<float> pointKNNSquaredDistance; |
| 48 | PointT searchPoint; |
| 49 | |
| 50 | // Search for nearby trees |
| 51 | searchPoint.x = pose.translation()[0]; |
| 52 | searchPoint.y = pose.translation()[1]; |
| 53 | searchPoint.z = 1; |
| 54 | if(kdtree.nearestKSearch(searchPoint, 100, |
| 55 | pointIdxKNNSearch, pointKNNSquaredDistance) > 0){ |
| 56 | |
| 57 | int idx_count = 0; |
| 58 | auto map_size = treeModels_.size(); |
| 59 | for(auto map_idx : pointIdxKNNSearch){ |
| 60 | if(map_size - map_idx < 200) |
| 61 | { |
| 62 | matchesMap.insert(std::pair<int, int>(idx_count, map_idx)); |
| 63 | submap.push_back(treeModels_[map_idx]); |
| 64 | idx_count++; |
| 65 | } |
| 66 | } |
| 67 | |
| 68 | } else { |
| 69 | ROS_INFO("Not enough landmarks around pose: Total: %ld", pointIdxKNNSearch.size()); |
| 70 | } |
| 71 | } |