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,29 +173,35 @@ 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(
+          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.
+    const transform::Rigid2d initial_relative_pose =
+        optimization_problem_.submap_data()
+            .at(submap_id.trajectory_id)
+            .at(submap_id.submap_index)
+            .pose.inverse() *
+        optimization_problem_.node_data()
+            .at(node_id.trajectory_id)
+            .at(node_id.node_index)
+            .pose;
+    constraint_builder_.MaybeAddConstraint(
+        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());
-  } else {
-    if (trajectory_connectivity_state_.TransitivelyConnected(
-            node_id.trajectory_id, submap_id.trajectory_id)) {
-      const transform::Rigid2d initial_relative_pose =
-          optimization_problem_.submap_data()
-              .at(submap_id.trajectory_id)
-              .at(submap_id.submap_index)
-              .pose.inverse() *
-          optimization_problem_.node_data()
-              .at(node_id.trajectory_id)
-              .at(node_id.node_index)
-              .pose;
-      constraint_builder_.MaybeAddConstraint(
-          submap_id, submap_data_.at(submap_id).submap.get(), node_id,
-          trajectory_nodes_.at(node_id).constant_data.get(),
-          initial_relative_pose);
-    }
   }
 }
 
@@ -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,33 +193,39 @@ 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(
-    // In this situation, 'initial_relative_pose' is:
+          node_id.trajectory_id, submap_id.trajectory_id);
-    //
+  if (node_id.trajectory_id == submap_id.trajectory_id ||
-    // submap <- global map 2 <- global map 1 <- tracking
+      scan_time <
-    //               (agreeing on gravity)
+          last_connection_time + common::FromSeconds(
-    //
+              options_.global_constraint_search_after_n_seconds())) {
-    // Since they possibly came from two disconnected trajectories, the only
+    // If the scan and the submap belong to the same trajectory or if there has
-    // guaranteed connection between the tracking and the submap frames is
+    // been a recent global constraint that ties that scan's trajectory to the
-    // an agreement on the direction of gravity. Therefore, excluding yaw,
+    // submap's trajectory, it suffices to do a match constrained to a local
-    // 'initial_relative_pose.rotation()' is a good estimate of the relative
+    // search window.
-    // orientation of the point cloud in the submap frame. Finding the correct
+    constraint_builder_.MaybeAddConstraint(
-    // yaw component will be handled by the matching procedure in the
-    // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
-    constraint_builder_.MaybeAddGlobalConstraint(
         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());
+        initial_relative_pose);
-  } else {
+  } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
-    if (trajectory_connectivity_state_.TransitivelyConnected(
+      // In this situation, 'initial_relative_pose' is:
-            node_id.trajectory_id, submap_id.trajectory_id)) {
+      //
-      constraint_builder_.MaybeAddConstraint(
+      // submap <- global map 2 <- global map 1 <- tracking
+      //               (agreeing on gravity)
+      //
+      // Since they possibly came from two disconnected trajectories, the only
+      // guaranteed connection between the tracking and the submap frames is
+      // an agreement on the direction of gravity. Therefore, excluding yaw,
+      // 'initial_relative_pose.rotation()' is a good estimate of the relative
+      // orientation of the point cloud in the submap frame. Finding the correct
+      // yaw component will be handled by the matching procedure in the
+      // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
+      constraint_builder_.MaybeAddGlobalConstraint(
           submap_id, submap_data_.at(submap_id).submap.get(), node_id,
           trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
-          initial_relative_pose);
+          initial_relative_pose.rotation());
-    }
   }
 }
 
@@ -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.,
 }