Enhance RANSAC Alignment with Inliers, Keypoint Pairs, and Alignment Time Visualization

cpp/map_closures/AlignRansac2D.cpp

@@ -74,7 +74,8 @@ namespace map_closures {
 
 PointPair::PointPair(const Eigen::Vector2d &r, const Eigen::Vector2d &q) : ref(r), query(q) {}
 
-std::pair<Eigen::Isometry2d, int> RansacAlignment2D(const std::vector<PointPair> &keypoint_pairs) {
+std::pair<Eigen::Isometry2d, std::vector<PointPair>> RansacAlignment2D(
+    const std::vector<PointPair> &keypoint_pairs) {
     const size_t max_inliers = keypoint_pairs.size();
 
     std::vector<PointPair> sample_keypoint_pairs(2);
@@ -112,6 +113,6 @@ std::pair<Eigen::Isometry2d, int> RansacAlignment2D(const std::vector<PointPair>
                    inlier_keypoint_pairs.begin(),
                    [&](const auto index) { return keypoint_pairs[index]; });
     auto T = KabschUmeyamaAlignment2D(inlier_keypoint_pairs);
-    return {T, num_inliers};
+    return {T, inlier_keypoint_pairs};
 }
 }  // namespace map_closures

cpp/map_closures/AlignRansac2D.hpp

@@ -37,5 +37,6 @@ struct PointPair {
     Eigen::Vector2d query = Eigen::Vector2d::Zero();
 };
 
-std::pair<Eigen::Isometry2d, int> RansacAlignment2D(const std::vector<PointPair> &keypoint_pairs);
+std::pair<Eigen::Isometry2d, std::vector<PointPair>> RansacAlignment2D(
+    const std::vector<PointPair> &keypoint_pairs);
 }  // namespace map_closures

cpp/map_closures/MapClosures.cpp

@@ -28,6 +28,7 @@
 
 #include <Eigen/Core>
 #include <algorithm>
+#include <chrono>
 #include <cmath>
 #include <opencv2/core.hpp>
 #include <utility>
@@ -119,12 +120,31 @@ ClosureCandidate MapClosures::ValidateClosure(const int reference_id, const int
                 return PointPair(ref_point, query_point);
             });
 
-        const auto &[pose2d, number_of_inliers] = RansacAlignment2D(keypoint_pairs);
+        const auto start = std::chrono::high_resolution_clock::now();
+        const auto &[pose2d, inliers] = RansacAlignment2D(keypoint_pairs);
+        const auto end = std::chrono::high_resolution_clock::now();
         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.number_of_inliers = number_of_inliers;
+        closure.number_of_inliers = inliers.size();
+        closure.alignment_time = std::chrono::duration<double, std::milli>(end - start).count();
+        closure.keypoint_pairs.reserve(keypoint_pairs.size());
+        closure.inliers.reserve(inliers.size());
+        auto to_map_point = [](const auto &p, const auto &offset) {
+            return Eigen::Vector2d(p.y() - offset.y(), p.x() - offset.x());
+        };
+        std::transform(keypoint_pairs.cbegin(), keypoint_pairs.cend(),
+                       std::back_inserter(closure.keypoint_pairs), [&](const PointPair &pair) {
+                           return std::make_pair(to_map_point(pair.ref, ref_map_lower_bound),
+                                                 to_map_point(pair.query, qry_map_lower_bound));
+                       });
+
+        std::transform(inliers.cbegin(), inliers.cend(), std::back_inserter(closure.inliers),
+                       [&](const PointPair &pair) {
+                           return std::make_pair(to_map_point(pair.ref, ref_map_lower_bound),
+                                                 to_map_point(pair.query, qry_map_lower_bound));
+                       });
     }
     return closure;
 }

cpp/map_closures/MapClosures.hpp

@@ -51,6 +51,9 @@ struct ClosureCandidate {
     int target_id = -1;
     Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
     size_t number_of_inliers = 0;
+    std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> keypoint_pairs;
+    std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> inliers;
+    double alignment_time = 0.0;
 };
 
 class MapClosures {

python/map_closures/pipeline.py

@@ -177,6 +177,8 @@ def _run_pipeline(self):
                             reference_local_map.scan_indices[0],
                             query_local_map.scan_indices[0],
                             closure.pose.flatten(),
+                            closure.number_of_inliers,
+                            closure.alignment_time,
                         ]
                     )
 
@@ -190,7 +192,7 @@ def _run_pipeline(self):
                         )
 
                     self.visualizer.update_closures(
-                        np.asarray(closure.pose), [closure.source_id, closure.target_id]
+                        np.asarray(closure.pose), [closure.source_id, closure.target_id], closure.keypoint_pairs, closure.inliers, closure.alignment_time
                     )
 
                 self.voxel_local_map.remove_far_away_points(frame_to_map_pose[:3, -1])
@@ -230,6 +232,8 @@ def _log_to_console(self):
         table.add_column("Query Map Index", justify="left", style="magenta")
         table.add_column("Relative Translation 2D", justify="right", style="green")
         table.add_column("Relative Rotation 2D", justify="right", style="green")
+        table.add_column("Inliers", justify="right", style="green")
+        table.add_column("Alignment Time", justify="right", style="green")
 
         for i, closure in enumerate(self.closures):
             table.add_row(
@@ -238,6 +242,8 @@ def _log_to_console(self):
                 f"{int(closure[1])}",
                 f"[{closure[7]:.4f}, {closure[11]:.4f}] m",
                 f"{(np.arctan2(closure[8], closure[9]) * 180 / np.pi):.4f} deg",
+                f"{int(closure[20])}",
+                f"{closure[21]:.4f} ms",
             )
         console.print(table)
 

python/map_closures/pybind/map_closures_pybind.cpp

@@ -61,7 +61,10 @@ PYBIND11_MODULE(map_closures_pybind, m) {
         .def_readwrite("source_id", &ClosureCandidate::source_id)
         .def_readwrite("target_id", &ClosureCandidate::target_id)
         .def_readwrite("pose", &ClosureCandidate::pose)
-        .def_readwrite("number_of_inliers", &ClosureCandidate::number_of_inliers);
+        .def_readwrite("number_of_inliers", &ClosureCandidate::number_of_inliers)
+        .def_readwrite("keypoint_pairs", &ClosureCandidate::keypoint_pairs)
+        .def_readwrite("inliers", &ClosureCandidate::inliers)
+        .def_readwrite("alignment_time", &ClosureCandidate::alignment_time);
 
     py::class_<MapClosures, std::shared_ptr<MapClosures>> map_closures(m, "_MapClosures", "");
     map_closures

python/map_closures/visualizer/closures_visualizer.py

@@ -24,6 +24,32 @@
 
 import numpy as np
 from matplotlib import pyplot as plt
+from matplotlib.patches import ConnectionPatch
+from matplotlib.backend_tools import ToolToggleBase
+import warnings
+
+# Ignore only the specific warning about the toolbar
+warnings.filterwarnings(
+    "ignore", 
+    message="Treat the new Tool classes introduced in v1.5 as experimental for now; the API and rcParam may change in future versions."
+)
+plt.rcParams['toolbar'] = 'toolmanager'
+
+class ToggleOutliersTool(ToolToggleBase):
+    """
+    Tool to toggle the visibility of outliers in the density maps viewer.
+    """
+    default_keymap = 'O'
+    description = 'Toggle Outliers'
+    image = r"icons/outliers.png"
+
+    def __init__(self, *args, visualizer, **kwargs):
+        super().__init__(*args, **kwargs)
+        self.visualizer = visualizer
+
+    def trigger(self, sender, event, data=None):
+        self.visualizer.show_outliers = not self.visualizer.show_outliers
+        self.visualizer.update_connection_visibility()
 
 # Button names
 PREV_CLOSURE = "Prev Closure [P]"
@@ -49,6 +75,9 @@ class LoopClosureData:
     current_id: int = 0
     closure_edges: list = field(default_factory=list)
     alignment_pose: list = field(default_factory=list)
+    keypoints_pairs: list = field(default_factory=list)
+    inliers: list = field(default_factory=list)
+    alignment_time: list = field(default_factory=list)
 
 
 @dataclass
@@ -79,8 +108,13 @@ def __init__(self, ps, gui, localmap_data):
         # Create data
         self.localmap_data = localmap_data
         self.data = LoopClosureData()
+
+        # Initialize connections list for density map viewer
+        self.connections = []
+        self.show_outliers = False
+        self.update_connection_visibility = None
 
-    def update_closures(self, alignment_pose, closure_edge):
+    def update_closures(self, alignment_pose, closure_edge, keypoints_pairs, inliers, alignment_time):
         self.data.closure_edges.append(closure_edge)
         self.data.alignment_pose.append(alignment_pose)
         self.data.size += 1
@@ -89,6 +123,9 @@ def update_closures(self, alignment_pose, closure_edge):
             self._register_trajectory()
         if self.states.toggle_view:
             self._render_closure()
+        self.data.keypoints_pairs.append(keypoints_pairs)
+        self.data.inliers.append(inliers)
+        self.data.alignment_time.append(alignment_time)
 
     def matplotlib_eventloop(self):
         plt.gcf().canvas.draw()
@@ -161,6 +198,9 @@ def _density_map_callback(self):
                 [ref_id, query_id] = self.data.closure_edges[id]
                 plt.ion()
                 self.fig = plt.figure()
+                self.update_connection_visibility = self._update_connection_visibility
+                self.fig.canvas.manager.toolmanager.add_tool('ToggleOutliers', ToggleOutliersTool, visualizer=self)
+                self.fig.canvas.manager.toolbar.add_tool('ToggleOutliers', 'toolgroup')
                 plt.show(block=False)
                 ax_ref = self.fig.add_subplot(1, 2, 1)
                 ax_ref.set_title("Reference Density Map")
@@ -172,9 +212,61 @@ def _density_map_callback(self):
                 self.query_density_viewer = ax_query.imshow(
                     self.localmap_data.density_maps[query_id], cmap="gray"
                 )
+                keypoints_pairs = self.data.keypoints_pairs[id]
+                inliers = set((tuple(ref_kp), tuple(query_kp)) for ref_kp, query_kp in self.data.inliers[id])
+                alignment_time = self.data.alignment_time[id]
+                self.fig.suptitle(f"Alignment Time: {alignment_time:.2f}ms, Inliers: {len(inliers)}")
+
+                def is_within_limits(point, ax):
+                    xlim = ax.get_xlim()
+                    ylim = ax.get_ylim()
+                    return xlim[0] <= point[0] <= xlim[1] and ylim[1] <= point[1] <= ylim[0]
+
+                def update_connection_visibility(event=None):
+                    for con, ref_kp, query_kp, is_inlier in self.connections:
+                        ref_visible = is_within_limits(ref_kp, ax_ref)
+                        query_visible = is_within_limits(query_kp, ax_query)
+                        con.set_visible(ref_visible and query_visible and (is_inlier or self.show_outliers))
+                    self.fig.canvas.draw_idle()
+
+                self.update_connection_visibility = update_connection_visibility
+
+                for ref_kp, query_kp in keypoints_pairs:
+                    is_inlier = (tuple(ref_kp), tuple(query_kp)) in inliers
+                    if is_inlier:
+                        color = 'g'
+                        marker = 'o'
+                        markersize = 5
+                        linewidth = 1
+                    else:
+                        color = 'r'
+                        marker = 'x'
+                        markersize = 2
+                        linewidth = 0.5
+                    ax_ref.plot(ref_kp[0], ref_kp[1], marker, color=color, markersize=markersize)
+                    ax_query.plot(query_kp[0], query_kp[1], marker, color=color, markersize=markersize)
+                    con = ConnectionPatch(
+                        xyA=ref_kp, coordsA=ax_ref.transData,
+                        xyB=query_kp, coordsB=ax_query.transData,
+                        color=color, linewidth=linewidth, linestyle='dashed'
+                    )
+                    con.set_visible(True)
+                    self.fig.add_artist(con)
+                    self.connections.append((con, ref_kp, query_kp, is_inlier))
+
+                ax_ref.callbacks.connect('xlim_changed', update_connection_visibility)
+                ax_ref.callbacks.connect('ylim_changed', update_connection_visibility)
+                ax_query.callbacks.connect('xlim_changed', update_connection_visibility)
+                ax_query.callbacks.connect('ylim_changed', update_connection_visibility)
+
+                update_connection_visibility()
                 self.matplotlib_eventloop()
             else:
                 plt.close(self.fig)
+
+    def _update_connection_visibility(self):
+        if self.update_connection_visibility:
+            self.update_connection_visibility()
 
     def _render_closure(self):
         id = self.data.current_id

python/map_closures/visualizer/visualizer.py

@@ -95,8 +95,8 @@ def update_data(self, local_map, density_map, local_map_pose):
         self.localmap_data.density_maps.append(density_map)
         self.localmap_data.local_map_poses.append(local_map_pose)
 
-    def update_closures(self, alignment_pose, closure_edge):
-        self.closures.update_closures(alignment_pose, closure_edge)
+    def update_closures(self, alignment_pose, closure_edge, keypoints_pairs, inliers, alignment_time):
+        self.closures.update_closures(alignment_pose, closure_edge, keypoints_pairs, inliers, alignment_time)
 
     def _initialize_visualizers(self):
         self._ps.set_program_name("MapClosures Visualizer")