MIT-SPARK · GoldenZephyr · Nov 20, 2025 · Mar 23, 2025 · Nov 20, 2025 · Nov 20, 2025
diff --git a/include/hydra/backend/external_loop_closure_receiver.h b/include/hydra/backend/external_loop_closure_receiver.h
@@ -39,17 +39,25 @@
 #include <spark_dsg/dynamic_scene_graph.h>
 
 #include <list>
-#include <optional>
 
 #include "hydra/common/message_queue.h"
 
 namespace hydra {
 
+struct KeyFromComparator {
+  bool operator()(pose_graph_tools::PoseGraphEdge a,
+                  pose_graph_tools::PoseGraphEdge b) const {
+    return a.key_from < b.key_from;
+  }
+};
+
 class ExternalLoopClosureReceiver {
  public:
   using Queue = MessageQueue<pose_graph_tools::PoseGraph>;
   using Callback = std::function<void(
       spark_dsg::NodeId to, spark_dsg::NodeId from, const gtsam::Pose3 to_T_from)>;
+  using OrderedPreviousLoops =
+      std::set<pose_graph_tools::PoseGraphEdge, KeyFromComparator>;
 
   struct LookupResult {
     enum class Status {
@@ -66,6 +74,14 @@ class ExternalLoopClosureReceiver {
     //! Maximum difference in seconds between a loop closure timestamp and the nearest
     //! agent pose. 0 disables checking time differences.
     double max_time_difference = 1.0;
+    //! Time window where a loop closure may block adding future loop closures
+    double lockout_time = 5.0;
+    //! Threshold above which a loop closure constraining equivalent poses is considered
+    //! unique
+    double min_pose_discrepancy = 2.0;
+    //! Weighting of rotation component of pose discrepency calculation
+    double rotation_scale = 1.0;
+
   } const config;
 
   ExternalLoopClosureReceiver(const Config& config, Queue* const queue);
@@ -75,9 +91,17 @@ class ExternalLoopClosureReceiver {
                            int robot_id,
                            double max_diff_s = 0.0) const;
 
+  OrderedPreviousLoops& getPreviousLoopsForRobotPair(size_t, size_t);
+
  protected:
+  bool should_add_lc(const OrderedPreviousLoops& added_lcs,
+                     const uint64_t stamp_ns_from,
+                     const uint64_t stamp_ns_to,
+                     const Eigen::Affine3d& to_T_from);
   Queue* const input_queue_;
   std::list<pose_graph_tools::PoseGraphEdge> loop_closures_;
+  // Maps robot ID pairs to previous loop closures between those robots
+  std::map<std::pair<size_t, size_t>, OrderedPreviousLoops> added_loop_closures_;
 };
 
 void declare_config(ExternalLoopClosureReceiver::Config& config);

diff --git a/src/backend/external_loop_closure_receiver.cpp b/src/backend/external_loop_closure_receiver.cpp
@@ -76,6 +76,18 @@ void declare_config(ExternalLoopClosureReceiver::Config& config) {
   field(config.layer, "layer");
   field(config.max_time_difference, "max_time_difference", "s");
   check(config.max_time_difference, GE, 0.0, "max_time_difference");
+  field(config.lockout_time, "lockout_time_ns", "s");
+  field(config.min_pose_discrepancy, "min_pose_discrepancy", "m");
+  field(config.rotation_scale, "rotation_scale");
+}
+
+double computePoseDiff(const Eigen::Affine3d& pose1,
+                       const Eigen::Affine3d& pose2,
+                       const double rotation_scale) {
+  Eigen::Affine3d pose_discrepancy = pose1.inverse() * pose2;
+  double translation_diff = pose_discrepancy.translation().norm();
+  double rotation_diff = std::acos((pose_discrepancy.rotation().trace() - 1) / 2);
+  return translation_diff + rotation_scale * abs(rotation_diff);
 }
 
 ExternalLoopClosureReceiver::ExternalLoopClosureReceiver(const Config& config,
@@ -84,6 +96,17 @@ ExternalLoopClosureReceiver::ExternalLoopClosureReceiver(const Config& config,
   LOG_IF(WARNING, !input_queue_) << "External loop closures disabled!";
 }
 
+ExternalLoopClosureReceiver::OrderedPreviousLoops&
+ExternalLoopClosureReceiver::getPreviousLoopsForRobotPair(size_t robot_a,
+                                                          size_t robot_b) {
+  std::pair<size_t, size_t> key = {std::min(robot_a, robot_b),
+                                   std::max(robot_a, robot_b)};
+  if (!added_loop_closures_.count(key)) {
+    added_loop_closures_.insert({key, {}});
+  }
+  return added_loop_closures_.at(key);
+}
+
 LookupResult ExternalLoopClosureReceiver::findClosest(const DynamicSceneGraph& graph,
                                                       uint64_t stamp_ns,
                                                       int robot_id,
@@ -134,6 +157,59 @@ LookupResult ExternalLoopClosureReceiver::findClosest(const DynamicSceneGraph& g
   return {LookupResult::Status::VALID, best_id};
 }
 
+bool ExternalLoopClosureReceiver::should_add_lc(const OrderedPreviousLoops& added_lcs,
+                                                const uint64_t stamp_ns_from,
+                                                const uint64_t stamp_ns_to,
+                                                const Eigen::Affine3d& to_T_from) {
+  size_t lockout_time_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+                               std::chrono::duration<double>(config.lockout_time))
+                               .count();
+  double min_pose_discrepancy = config.min_pose_discrepancy;
+
+  auto pge_stamp_cmp = [](const pose_graph_tools::PoseGraphEdge& pge,
+                          const uint64_t value) { return pge.key_from < value; };
+
+  uint64_t stamp_from_lower;
+  if (lockout_time_ns > stamp_ns_from) {
+    stamp_from_lower = 0;
+  } else {
+    stamp_from_lower = stamp_ns_from - lockout_time_ns;
+  }
+  auto lc_iter = std::lower_bound(
+      added_lcs.begin(), added_lcs.end(), stamp_from_lower, pge_stamp_cmp);
+
+  uint64_t stamp_to_lower;
+  if (lockout_time_ns > stamp_ns_to) {
+    stamp_to_lower = 0;
+  } else {
+    stamp_to_lower = stamp_ns_to - lockout_time_ns;
+  }
+
+  uint64_t stamp_to_upper = stamp_ns_to + lockout_time_ns;
+
+  while (lc_iter != added_lcs.end()) {
+    uint64_t key_to = lc_iter->key_to;
+    if (lc_iter->key_from > stamp_ns_from + lockout_time_ns) {
+      break;
+    }
+    if (key_to < stamp_to_lower || key_to > stamp_to_upper) {
+      ++lc_iter;
+      continue;
+    }
+
+    // The putative loop closure constrains approximately the same poses as an
+    // existing loop closure. We still add the new loop closure if the relative pose
+    // measurement is different enough from the existing one.
+
+    double pose_diff = computePoseDiff(to_T_from, lc_iter->pose, config.rotation_scale);
+    if (pose_diff < min_pose_discrepancy) {
+      return false;
+    }
+    ++lc_iter;
+  }
+  return true;
+}
+
 void ExternalLoopClosureReceiver::update(const DynamicSceneGraph& graph,
                                          const Callback& callback) {
   if (!input_queue_) {
@@ -171,8 +247,17 @@ void ExternalLoopClosureReceiver::update(const DynamicSceneGraph& graph,
       continue;
     }
 
-    // to_id, from_id, to_T_from
-    callback(to_node.id, from_node.id, gtsam::Pose3(edge.pose.matrix()));
+    const auto to_ns = getAgentTimestamp(graph.getNode(to_node.id)).count();
+    const auto from_ns = getAgentTimestamp(graph.getNode(from_node.id)).count();
+    auto& previous_loops_for_pair =
+        getPreviousLoopsForRobotPair(edge.robot_from, edge.robot_to);
+    bool should_add = should_add_lc(previous_loops_for_pair, from_ns, to_ns, edge.pose);
+
+    if (should_add) {
+      previous_loops_for_pair.insert(edge);
+      // to_id, from_id, to_T_from
+      callback(to_node.id, from_node.id, gtsam::Pose3(edge.pose.matrix()));
+    }
     iter = loop_closures_.erase(iter);
   }
 }