massive change in API: replace match_and_add function with get_closures

saurabh1002 · saurabh1002 · commit 05a4f4c7224f · 2025-01-20T14:56:43.000+01:00
diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp
@@ -67,7 +67,8 @@ MapClosures::MapClosures(const Config &config) : config_(config) {
                         cv::ORB::ScoreType(score_type), patch_size, fast_threshold);
 }
 
-void MapClosures::MatchAndAdd(const int id, const std::vector<Eigen::Vector3d> &local_map) {
+void MapClosures::MatchAndAddToDatabase(const int id,
+                                        const std::vector<Eigen::Vector3d> &local_map) {
     Eigen::Matrix4d T_ground = AlignToLocalGround(local_map, ground_alignment_resolution);
     DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution,
                                                 config_.density_threshold);
@@ -104,9 +105,37 @@ void MapClosures::MatchAndAdd(const int id, const std::vector<Eigen::Vector3d> &
                                    srrg_hbst::SplittingStrategy::SplitEven);
 }
 
+ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int query_id) const {
+    const Tree::MatchVector &matches = descriptor_matches_.at(reference_id);
+    const size_t num_matches = matches.size();
+
+    ClosureCandidate closure;
+    if (num_matches > min_no_of_matches) {
+        std::vector<PointPair> keypoint_pairs(num_matches);
+        std::transform(matches.cbegin(), matches.cend(), keypoint_pairs.begin(),
+                       [&](const Tree::Match &match) {
+                           auto query_point =
+                               Eigen::Vector2d(match.object_query.pt.y, match.object_query.pt.x);
+                           auto ref_point = Eigen::Vector2d(match.object_references[0].pt.y,
+                                                            match.object_references[0].pt.x);
+                           return PointPair(ref_point, query_point);
+                       });
+
+        const auto &[pose2d, number_of_inliers] = RansacAlignment2D(keypoint_pairs);
+        closure.source_id = reference_id;
+        closure.target_id = query_id;
+        closure.pose.block<2, 2>(0, 0) = pose2d.linear();
+        closure.pose.block<2, 1>(0, 3) = pose2d.translation() * config_.density_map_resolution;
+        closure.pose = ground_alignments_.at(query_id).inverse() * closure.pose *
+                       ground_alignments_.at(reference_id);
+        closure.number_of_inliers = number_of_inliers;
+    }
+    return closure;
+}
+
 ClosureCandidate MapClosures::GetBestClosure(const int query_id,
                                              const std::vector<Eigen::Vector3d> &local_map) {
-    MatchAndAdd(query_id, local_map);
+    MatchAndAddToDatabase(query_id, local_map);
     auto compare_closure_candidates = [](ClosureCandidate a,
                                          const ClosureCandidate &b) -> ClosureCandidate {
         return a.number_of_inliers > b.number_of_inliers ? a : b;
@@ -126,7 +155,7 @@ ClosureCandidate MapClosures::GetBestClosure(const int query_id,
 
 std::vector<ClosureCandidate> MapClosures::GetTopKClosures(
     const int query_id, const std::vector<Eigen::Vector3d> &local_map, const int k) {
-    MatchAndAdd(query_id, local_map);
+    MatchAndAddToDatabase(query_id, local_map);
     auto compare_closure_candidates = [](const ClosureCandidate &a, const ClosureCandidate &b) {
         return a.number_of_inliers >= b.number_of_inliers;
     };
@@ -156,32 +185,4 @@ std::vector<ClosureCandidate> MapClosures::GetTopKClosures(
                 std::back_inserter(top_k_closures));
     return top_k_closures;
 }
-
-ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int query_id) const {
-    const Tree::MatchVector &matches = descriptor_matches_.at(reference_id);
-    const size_t num_matches = matches.size();
-
-    ClosureCandidate closure;
-    if (num_matches > min_no_of_matches) {
-        std::vector<PointPair> keypoint_pairs(num_matches);
-        std::transform(matches.cbegin(), matches.cend(), keypoint_pairs.begin(),
-                       [&](const Tree::Match &match) {
-                           auto query_point =
-                               Eigen::Vector2d(match.object_query.pt.y, match.object_query.pt.x);
-                           auto ref_point = Eigen::Vector2d(match.object_references[0].pt.y,
-                                                            match.object_references[0].pt.x);
-                           return PointPair(ref_point, query_point);
-                       });
-
-        const auto &[pose2d, number_of_inliers] = RansacAlignment2D(keypoint_pairs);
-        closure.source_id = reference_id;
-        closure.target_id = query_id;
-        closure.pose.block<2, 2>(0, 0) = pose2d.linear();
-        closure.pose.block<2, 1>(0, 3) = pose2d.translation() * config_.density_map_resolution;
-        closure.pose = ground_alignments_.at(query_id).inverse() * closure.pose *
-                       ground_alignments_.at(reference_id);
-        closure.number_of_inliers = number_of_inliers;
-    }
-    return closure;
-}
 }  // namespace map_closures
diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp
@@ -47,7 +47,6 @@ struct Config {
 };
 
 struct ClosureCandidate {
-    ClosureCandidate() = default;
     int source_id = -1;
     int target_id = -1;
     Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
@@ -74,8 +73,8 @@ class MapClosures {
     }
 
 protected:
+    void MatchAndAddToDatabase(const int id, const std::vector<Eigen::Vector3d> &local_map);
     ClosureCandidate ValidateClosure(const int reference_id, const int query_id) const;
-    void MatchAndAdd(const int id, const std::vector<Eigen::Vector3d> &local_map);
 
     Config config_;
     Tree::MatchVectorMap descriptor_matches_;
diff --git a/python/map_closures/map_closures.py b/python/map_closures/map_closures.py
@@ -36,20 +36,21 @@ def __init__(self, config: MapClosuresConfig = MapClosuresConfig()):
         self._config = config
         self._pipeline = map_closures_pybind._MapClosures(self._config.model_dump())
 
-    def match_and_add(self, map_idx: int, local_map: np.ndarray):
+    def get_best_closures(self, query_idx: int, local_map: np.ndarray) -> ClosureCandidate:
         pcd = map_closures_pybind._Vector3dVector(local_map)
-        self._pipeline._MatchAndAdd(map_idx, pcd)
-
-    def get_best_closures(self, query_idx: int) -> ClosureCandidate:
-        closure = self._pipeline._GetBestClosure(query_idx)
+        closure = self._pipeline._GetBestClosure(query_idx, pcd)
         return closure
 
-    def get_top_k_closures(self, query_idx: int, k: int) -> List[ClosureCandidate]:
-        top_k_closures = self._pipeline._GetTopKClosures(query_idx, k)
+    def get_top_k_closures(
+        self, query_idx: int, local_map: np.ndarray, k: int
+    ) -> List[ClosureCandidate]:
+        pcd = map_closures_pybind._Vector3dVector(local_map)
+        top_k_closures = self._pipeline._GetTopKClosures(query_idx, pcd, k)
         return top_k_closures
 
-    def get_closures(self, query_idx: int) -> List[ClosureCandidate]:
-        closures = self._pipeline._GetClosures(query_idx)
+    def get_closures(self, query_idx: int, local_map: np.ndarray) -> List[ClosureCandidate]:
+        pcd = map_closures_pybind._Vector3dVector(local_map)
+        closures = self._pipeline._GetClosures(query_idx, pcd)
         return closures
 
     def get_density_map_from_id(self, map_id: int) -> np.ndarray:
diff --git a/python/map_closures/pipeline.py b/python/map_closures/pipeline.py
@@ -153,7 +153,7 @@ def _run_pipeline(self):
                 scan_idx == self._n_scans - 1
             ):
                 local_map_pointcloud = self.voxel_local_map.point_cloud()
-                self.map_closures.match_and_add(map_idx, local_map_pointcloud)
+                closures = self.map_closures.get_closures(map_idx, local_map_pointcloud)
 
                 scan_indices_in_local_map.append(scan_idx)
                 poses_in_local_map.append(current_frame_pose)
@@ -171,7 +171,6 @@ def _run_pipeline(self):
                     self.local_maps[-1].density_map,
                     current_map_pose,
                 )
-                closures = self.map_closures.get_closures(map_idx)
                 for closure in closures:
                     if closure.number_of_inliers > self.closure_config.inliers_threshold:
                         reference_local_map = self.local_maps[closure.source_id]
diff --git a/python/map_closures/pybind/map_closures_pybind.cpp b/python/map_closures/pybind/map_closures_pybind.cpp
@@ -77,9 +77,8 @@ PYBIND11_MODULE(map_closures_pybind, m) {
                  cv::cv2eigen(density_map.grid, density_map_eigen);
                  return density_map_eigen;
              })
-        .def("_MatchAndAdd", &MapClosures::MatchAndAdd, "map_id"_a, "local_map"_a)
-        .def("_GetBestClosure", &MapClosures::GetBestClosure, "query_id"_a)
-        .def("_GetTopKClosures", &MapClosures::GetTopKClosures, "query_id"_a, "k"_a)
-        .def("_GetClosures", &MapClosures::GetClosures, "query_id"_a);
+        .def("_GetBestClosure", &MapClosures::GetBestClosure, "query_id"_a, "local_map"_a)
+        .def("_GetTopKClosures", &MapClosures::GetTopKClosures, "query_id"_a, "local_map"_a, "k"_a)
+        .def("_GetClosures", &MapClosures::GetClosures, "query_id"_a, "local_map"_a);
 }
 }  // namespace map_closures
