| 67 | } |
| 68 | |
| 69 | void MapClosures::MatchAndAddToDatabase(const int id, |
| 70 | const std::vector<Eigen::Vector3d> &local_map) { |
| 71 | Eigen::Matrix4d T_ground = AlignToLocalGround(local_map, config_.density_map_resolution); |
| 72 | DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, |
| 73 | config_.density_threshold); |
| 74 | cv::Mat orb_descriptors; |
| 75 | std::vector<cv::KeyPoint> orb_keypoints; |
| 76 | orb_keypoints.reserve(nfeatures); |
| 77 | orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, |
| 78 | orb_descriptors); |
| 79 | |
| 80 | std::vector<std::vector<cv::DMatch>> self_matches; |
| 81 | self_matches.reserve(orb_keypoints.size()); |
| 82 | self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); |
| 83 | |
| 84 | std::vector<Matchable *> hbst_matchable; |
| 85 | hbst_matchable.reserve(orb_descriptors.rows); |
| 86 | std::for_each( |
| 87 | self_matches.cbegin(), self_matches.cend(), [&](const std::vector<cv::DMatch> &self_match) { |
| 88 | if (self_match[1].distance > self_similarity_threshold) { |
| 89 | const int index_descriptor = self_match[0].queryIdx; |
| 90 | cv::KeyPoint keypoint = orb_keypoints[index_descriptor]; |
| 91 | keypoint.pt.x = keypoint.pt.x + static_cast<float>(density_map.lower_bound.y()); |
| 92 | keypoint.pt.y = keypoint.pt.y + static_cast<float>(density_map.lower_bound.x()); |
| 93 | hbst_matchable.emplace_back( |
| 94 | new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); |
| 95 | } |
| 96 | }); |
| 97 | |
| 98 | hbst_binary_tree_->matchAndAdd(hbst_matchable, descriptor_matches_, |
| 99 | config_.hamming_distance_threshold, |
| 100 | srrg_hbst::SplittingStrategy::SplitEven); |
| 101 | |
| 102 | density_maps_.emplace(id, std::move(density_map)); |
| 103 | ground_alignments_.emplace(id, std::move(T_ground)); |
| 104 | } |
| 105 | |
| 106 | void MapClosures::Match(const std::vector<Eigen::Vector3d> &local_map) { |
| 107 | const Eigen::Matrix4d T_ground = AlignToLocalGround(local_map, config_.density_map_resolution); |
nothing calls this directly
no test coverage detected