Introduce timeout for global constraints. (#517)

This PR introduces a new option that specifies the number of seconds
after which global matcher searches are performed if no recent global
constraints have been found between the submap's and the node's
trajectory.

cartographer/mapping/proto/sparse_pose_graph_options.proto

@@ -50,4 +50,9 @@ message SparsePoseGraphOptions {
 
   // Whether to output histograms for the pose residuals.
   optional bool log_residual_histograms = 9;
+
+  // If for the duration specified by this option no global contraint has been
+  // added between two trajectories, loop closure searches will be performed
+  // globally rather than in a smaller search window.
+  optional double global_constraint_search_after_n_seconds = 10;
 }

cartographer/mapping/sparse_pose_graph.cc

@@ -57,6 +57,9 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
       parameter_dictionary->GetDouble("global_sampling_ratio"));
   options.set_log_residual_histograms(
       parameter_dictionary->GetBool("log_residual_histograms"));
+  options.set_global_constraint_search_after_n_seconds(
+      parameter_dictionary->GetDouble(
+          "global_constraint_search_after_n_seconds"));
   return options;
 }
 

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -173,15 +173,18 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
                                         const mapping::SubmapId& submap_id) {
   CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
 
-  // Only globally match against submaps not in this trajectory.
+  const common::Time scan_time = GetLatestScanTime(node_id, submap_id);
-  if (node_id.trajectory_id != submap_id.trajectory_id &&
+  const common::Time last_connection_time =
-      global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
+      trajectory_connectivity_state_.LastConnectionTime(
-    constraint_builder_.MaybeAddGlobalConstraint(
+          node_id.trajectory_id, submap_id.trajectory_id);
-        submap_id, submap_data_.at(submap_id).submap.get(), node_id,
+  if (node_id.trajectory_id == submap_id.trajectory_id ||
-        trajectory_nodes_.at(node_id).constant_data.get());
+      scan_time <
-  } else {
+          last_connection_time + common::FromSeconds(
-    if (trajectory_connectivity_state_.TransitivelyConnected(
+              options_.global_constraint_search_after_n_seconds())) {
-            node_id.trajectory_id, submap_id.trajectory_id)) {
+    // If the scan and the submap belong to the same trajectory or if there has
+    // been a recent global constraint that ties that scan's trajectory to the
+    // submap's trajectory, it suffices to do a match constrained to a local
+    // search window.
     const transform::Rigid2d initial_relative_pose =
         optimization_problem_.submap_data()
             .at(submap_id.trajectory_id)
@@ -195,7 +198,10 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         submap_id, submap_data_.at(submap_id).submap.get(), node_id,
         trajectory_nodes_.at(node_id).constant_data.get(),
         initial_relative_pose);
-    }
+  } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
+    constraint_builder_.MaybeAddGlobalConstraint(
+        submap_id, submap_data_.at(submap_id).submap.get(), node_id,
+        trajectory_nodes_.at(node_id).constant_data.get());
   }
 }
 
@@ -300,21 +306,28 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   }
 }
 
+common::Time SparsePoseGraph::GetLatestScanTime(
+    const mapping::NodeId& node_id,
+    const mapping::SubmapId& submap_id) const {
+  common::Time time =
+      trajectory_nodes_.at(node_id).constant_data->time;
+  const SubmapData& submap_data = submap_data_.at(submap_id);
+  if (!submap_data.node_ids.empty()) {
+    const mapping::NodeId last_submap_node_id =
+        *submap_data_.at(submap_id).node_ids.rbegin();
+    time = std::max(
+        time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
+  }
+  return time;
+}
+
 void SparsePoseGraph::UpdateTrajectoryConnectivity(
     const sparse_pose_graph::ConstraintBuilder::Result& result) {
   for (const Constraint& constraint : result) {
     CHECK_EQ(constraint.tag,
              mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
-    CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
+    const common::Time time =
-    common::Time time =
+        GetLatestScanTime(constraint.node_id, constraint.submap_id);
-        trajectory_nodes_.at(constraint.node_id).constant_data->time;
-    const SubmapData& submap_data = submap_data_.at(constraint.submap_id);
-    if (!submap_data.node_ids.empty()) {
-      const mapping::NodeId last_submap_node_id =
-          *submap_data_.at(constraint.submap_id).node_ids.rbegin();
-      time = std::max(
-          time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
-    }
     trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
                                            constraint.submap_id.trajectory_id,
                                            time);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -99,6 +99,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
       override EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
+  common::Time GetLatestScanTime(const mapping::NodeId& node_id,
+                                 const mapping::SubmapId& submap_id) const
+      REQUIRES(mutex_);
 
  private:
   // The current state of the submap in the background threads. When this

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -130,6 +130,7 @@ class SparsePoseGraphTest : public ::testing::Test {
             max_num_final_iterations = 200,
             global_sampling_ratio = 0.01,
             log_residual_histograms = true,
+            global_constraint_search_after_n_seconds = 10.0,
           })text");
       sparse_pose_graph_ = common::make_unique<SparsePoseGraph>(
           mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()),

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -193,9 +193,23 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         inverse_submap_pose * trajectory_nodes_.at(submap_node_id).pose});
   }
 
-  // Only globally match against submaps not in this trajectory.
+  const common::Time scan_time = GetLatestScanTime(node_id, submap_id);
-  if (node_id.trajectory_id != submap_id.trajectory_id &&
+  const common::Time last_connection_time =
-      global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
+      trajectory_connectivity_state_.LastConnectionTime(
+          node_id.trajectory_id, submap_id.trajectory_id);
+  if (node_id.trajectory_id == submap_id.trajectory_id ||
+      scan_time <
+          last_connection_time + common::FromSeconds(
+              options_.global_constraint_search_after_n_seconds())) {
+    // If the scan and the submap belong to the same trajectory or if there has
+    // been a recent global constraint that ties that scan's trajectory to the
+    // submap's trajectory, it suffices to do a match constrained to a local
+    // search window.
+    constraint_builder_.MaybeAddConstraint(
+        submap_id, submap_data_.at(submap_id).submap.get(), node_id,
+        trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
+        initial_relative_pose);
+  } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
       // In this situation, 'initial_relative_pose' is:
       //
       // submap <- global map 2 <- global map 1 <- tracking
@@ -212,14 +226,6 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
           submap_id, submap_data_.at(submap_id).submap.get(), node_id,
           trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
           initial_relative_pose.rotation());
-  } else {
-    if (trajectory_connectivity_state_.TransitivelyConnected(
-            node_id.trajectory_id, submap_id.trajectory_id)) {
-      constraint_builder_.MaybeAddConstraint(
-          submap_id, submap_data_.at(submap_id).submap.get(), node_id,
-          trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
-          initial_relative_pose);
-    }
   }
 }
 
@@ -320,21 +326,28 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   }
 }
 
+common::Time SparsePoseGraph::GetLatestScanTime(
+    const mapping::NodeId& node_id,
+    const mapping::SubmapId& submap_id) const {
+  common::Time time =
+      trajectory_nodes_.at(node_id).constant_data->time;
+  const SubmapData& submap_data = submap_data_.at(submap_id);
+  if (!submap_data.node_ids.empty()) {
+    const mapping::NodeId last_submap_node_id =
+        *submap_data_.at(submap_id).node_ids.rbegin();
+    time = std::max(
+        time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
+  }
+  return time;
+}
+
 void SparsePoseGraph::UpdateTrajectoryConnectivity(
     const sparse_pose_graph::ConstraintBuilder::Result& result) {
   for (const Constraint& constraint : result) {
     CHECK_EQ(constraint.tag,
              mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
-    CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
+    const common::Time time =
-    common::Time time =
+        GetLatestScanTime(constraint.node_id, constraint.submap_id);
-        trajectory_nodes_.at(constraint.node_id).constant_data->time;
-    const SubmapData& submap_data = submap_data_.at(constraint.submap_id);
-    if (!submap_data.node_ids.empty()) {
-      const mapping::NodeId last_submap_node_id =
-          *submap_data_.at(constraint.submap_id).node_ids.rbegin();
-      time = std::max(
-          time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
-    }
     trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
                                            constraint.submap_id.trajectory_id,
                                            time);

cartographer/mapping_3d/sparse_pose_graph.h

@@ -164,6 +164,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
       const mapping::SubmapId& submap_id) REQUIRES(mutex_);
 
+  common::Time GetLatestScanTime(const mapping::NodeId& node_id,
+                                 const mapping::SubmapId& submap_id) const
+      REQUIRES(mutex_);
+
   // Logs histograms for the translational and rotational residual of scan
   // poses.
   void LogResidualHistograms() REQUIRES(mutex_);

configuration_files/sparse_pose_graph.lua

@@ -79,4 +79,5 @@ SPARSE_POSE_GRAPH = {
   max_num_final_iterations = 200,
   global_sampling_ratio = 0.003,
   log_residual_histograms = true,
+  global_constraint_search_after_n_seconds = 10.,
 }