| 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); |
| 108 | DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, |
| 109 | config_.density_threshold); |
| 110 | cv::Mat orb_descriptors; |
| 111 | std::vector<cv::KeyPoint> orb_keypoints; |
| 112 | orb_keypoints.reserve(nfeatures); |
| 113 | orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, |
| 114 | orb_descriptors); |
| 115 | |
| 116 | std::vector<std::vector<cv::DMatch>> self_matches; |
| 117 | self_matches.reserve(orb_keypoints.size()); |
| 118 | self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); |
| 119 | |
| 120 | std::vector<Matchable *> hbst_matchable; |
| 121 | hbst_matchable.reserve(orb_descriptors.rows); |
| 122 | std::for_each( |
| 123 | self_matches.cbegin(), self_matches.cend(), [&](const std::vector<cv::DMatch> &self_match) { |
| 124 | if (self_match[1].distance > self_similarity_threshold) { |
| 125 | const int index_descriptor = self_match[0].queryIdx; |
| 126 | cv::KeyPoint keypoint = orb_keypoints[index_descriptor]; |
| 127 | keypoint.pt.x = keypoint.pt.x + static_cast<float>(density_map.lower_bound.y()); |
| 128 | keypoint.pt.y = keypoint.pt.y + static_cast<float>(density_map.lower_bound.x()); |
| 129 | hbst_matchable.emplace_back( |
| 130 | new Matchable(keypoint, orb_descriptors.row(index_descriptor))); |
| 131 | } |
| 132 | }); |
| 133 | hbst_binary_tree_->match(hbst_matchable, descriptor_matches_, |
| 134 | config_.hamming_distance_threshold); |
| 135 | } |
| 136 | |
| 137 | ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int query_id) const { |
| 138 | const auto it = descriptor_matches_.find(reference_id); |
nothing calls this directly
no test coverage detected