| 135 | } |
| 136 | |
| 137 | ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int query_id) const { |
| 138 | const auto it = descriptor_matches_.find(reference_id); |
| 139 | if (it == descriptor_matches_.end()) { |
| 140 | return ClosureCandidate(); |
| 141 | } |
| 142 | |
| 143 | const Tree::MatchVector &matches = it->second; |
| 144 | const size_t num_matches = matches.size(); |
| 145 | |
| 146 | ClosureCandidate closure; |
| 147 | if (num_matches > min_no_of_matches) { |
| 148 | std::vector<PointPair> keypoint_pairs(num_matches); |
| 149 | std::transform(matches.cbegin(), matches.cend(), keypoint_pairs.begin(), |
| 150 | [&](const Tree::Match &match) { |
| 151 | const Eigen::Vector2d query_point = |
| 152 | Eigen::Vector2d(match.object_query.pt.y, match.object_query.pt.x); |
| 153 | const Eigen::Vector2d ref_point = Eigen::Vector2d( |
| 154 | match.object_references[0].pt.y, match.object_references[0].pt.x); |
| 155 | return PointPair(ref_point, query_point); |
| 156 | }); |
| 157 | |
| 158 | const auto [pose2d, number_of_inliers] = RansacAlignment2D(keypoint_pairs); |
| 159 | closure.source_id = reference_id; |
| 160 | closure.target_id = query_id; |
| 161 | closure.pose.block<2, 2>(0, 0) = pose2d.linear(); |
| 162 | closure.pose.block<2, 1>(0, 3) = pose2d.translation() * config_.density_map_resolution; |
| 163 | closure.pose = ground_alignments_.at(query_id).inverse() * closure.pose * |
| 164 | ground_alignments_.at(reference_id); |
| 165 | closure.number_of_inliers = number_of_inliers; |
| 166 | } |
| 167 | return closure; |
| 168 | } |
| 169 | |
| 170 | std::vector<ClosureCandidate> MapClosures::GetTopKClosures( |
| 171 | const int query_id, const std::vector<Eigen::Vector3d> &local_map, const int k) { |
nothing calls this directly
no test coverage detected