Fined-grained locking for PoseGraph3D constraints (#1279)

When preparing constraints make holding of the PoseGraph3D
mutex fine-grained to address lock contention from issue #1250.
That includes not holding the mutex when acquring the constraint
builder mutex.

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -245,8 +245,6 @@ void PoseGraph3D::AddLandmarkData(int trajectory_id,
 
 void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
                                     const SubmapId& submap_id) {
-  CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
-
   const transform::Rigid3d global_node_pose =
       optimization_problem_->node_data().at(node_id).global_pose;
 
@@ -256,57 +254,58 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
   const transform::Rigid3d global_submap_pose_inverse =
       global_submap_pose.inverse();
 
+  bool maybe_add_local_constraint = false;
+  bool maybe_add_global_constraint = false;
+  const TrajectoryNode::Data* constant_data;
+  const Submap3D* submap;
   std::vector<TrajectoryNode> submap_nodes;
-  for (const NodeId& submap_node_id :
+  {
-       data_.submap_data.at(submap_id).node_ids) {
+    common::MutexLocker locker(&mutex_);
-    submap_nodes.push_back(TrajectoryNode{
+    CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
-        data_.trajectory_nodes.at(submap_node_id).constant_data,
-        global_submap_pose_inverse *
-            data_.trajectory_nodes.at(submap_node_id).global_pose});
-  }
 
-  const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
+    for (const NodeId& submap_node_id :
-  const common::Time last_connection_time =
+         data_.submap_data.at(submap_id).node_ids) {
-      data_.trajectory_connectivity_state.LastConnectionTime(
+      submap_nodes.push_back(TrajectoryNode{
-          node_id.trajectory_id, submap_id.trajectory_id);
+          data_.trajectory_nodes.at(submap_node_id).constant_data,
-  if (node_id.trajectory_id == submap_id.trajectory_id ||
+          global_submap_pose_inverse *
-      node_time <
+              data_.trajectory_nodes.at(submap_node_id).global_pose});
-          last_connection_time +
-              common::FromSeconds(
-                  options_.global_constraint_search_after_n_seconds())) {
-    // If the node and the submap belong to the same trajectory or if there has
-    // been a recent global constraint that ties that node's trajectory to the
-    // submap's trajectory, it suffices to do a match constrained to a local
-    // search window.
-    constraint_builder_.MaybeAddConstraint(
-        submap_id,
-        static_cast<const Submap3D*>(
-            data_.submap_data.at(submap_id).submap.get()),
-        node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
-        submap_nodes, global_node_pose, global_submap_pose);
-  } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
-    // In this situation, 'global_node_pose' and 'global_submap_pose' have
-    // orientations agreeing on gravity. Their relationship regarding yaw is
-    // arbitrary. 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,
-        static_cast<const Submap3D*>(
-            data_.submap_data.at(submap_id).submap.get()),
-        node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
-        submap_nodes, global_node_pose.rotation(),
-        global_submap_pose.rotation());
-  }
-}
-
-void PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
-  const auto& submap_data = data_.submap_data.at(submap_id);
-  for (const auto& node_id_data : optimization_problem_->node_data()) {
-    const NodeId& node_id = node_id_data.id;
-    if (submap_data.node_ids.count(node_id) == 0) {
-      ComputeConstraint(node_id, submap_id);
     }
+
+    const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
+    const common::Time last_connection_time =
+        data_.trajectory_connectivity_state.LastConnectionTime(
+            node_id.trajectory_id, submap_id.trajectory_id);
+    if (node_id.trajectory_id == submap_id.trajectory_id ||
+        node_time <
+            last_connection_time +
+                common::FromSeconds(
+                    options_.global_constraint_search_after_n_seconds())) {
+      // If the node and the submap belong to the same trajectory or if there
+      // has been a recent global constraint that ties that node's trajectory to
+      // the submap's trajectory, it suffices to do a match constrained to a
+      // local search window.
+      maybe_add_local_constraint = true;
+    } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
+      // In this situation, 'global_node_pose' and 'global_submap_pose' have
+      // orientations agreeing on gravity. Their relationship regarding yaw is
+      // arbitrary. Finding the correct yaw component will be handled by the
+      // matching procedure in the FastCorrelativeScanMatcher, and the given yaw
+      // is essentially ignored.
+      maybe_add_global_constraint = true;
+    }
+    constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
+    submap = static_cast<const Submap3D*>(
+        data_.submap_data.at(submap_id).submap.get());
+  }
+
+  if (maybe_add_local_constraint) {
+    constraint_builder_.MaybeAddConstraint(
+        submap_id, submap, node_id, constant_data, submap_nodes,
+        global_node_pose, global_submap_pose);
+  } else if (maybe_add_global_constraint) {
+    constraint_builder_.MaybeAddGlobalConstraint(
+        submap_id, submap, node_id, constant_data, submap_nodes,
+        global_node_pose.rotation(), global_submap_pose.rotation());
   }
 }
 
@@ -314,53 +313,73 @@ WorkItem::Result PoseGraph3D::ComputeConstraintsForNode(
     const NodeId& node_id,
     std::vector<std::shared_ptr<const Submap3D>> insertion_submaps,
     const bool newly_finished_submap) {
-  common::MutexLocker locker(&mutex_);
+  std::vector<SubmapId> submap_ids;
-  const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data;
+  std::vector<SubmapId> finished_submap_ids;
-  const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
+  std::set<NodeId> newly_finished_submap_node_ids;
-      node_id.trajectory_id, constant_data->time, insertion_submaps);
+  {
-  CHECK_EQ(submap_ids.size(), insertion_submaps.size());
+    common::MutexLocker locker(&mutex_);
-  const SubmapId matching_id = submap_ids.front();
+    const auto& constant_data =
-  const transform::Rigid3d& local_pose = constant_data->local_pose;
+        data_.trajectory_nodes.at(node_id).constant_data;
-  const transform::Rigid3d global_pose =
+    submap_ids = InitializeGlobalSubmapPoses(
-      optimization_problem_->submap_data().at(matching_id).global_pose *
+        node_id.trajectory_id, constant_data->time, insertion_submaps);
-      insertion_submaps.front()->local_pose().inverse() * local_pose;
+    CHECK_EQ(submap_ids.size(), insertion_submaps.size());
-  optimization_problem_->AddTrajectoryNode(
+    const SubmapId matching_id = submap_ids.front();
-      matching_id.trajectory_id,
+    const transform::Rigid3d& local_pose = constant_data->local_pose;
-      optimization::NodeSpec3D{constant_data->time, local_pose, global_pose});
+    const transform::Rigid3d global_pose =
-  for (size_t i = 0; i < insertion_submaps.size(); ++i) {
+        optimization_problem_->submap_data().at(matching_id).global_pose *
-    const SubmapId submap_id = submap_ids[i];
+        insertion_submaps.front()->local_pose().inverse() * local_pose;
-    // Even if this was the last node added to 'submap_id', the submap will only
+    optimization_problem_->AddTrajectoryNode(
-    // be marked as finished in 'data_.submap_data' further below.
+        matching_id.trajectory_id,
-    CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive);
+        optimization::NodeSpec3D{constant_data->time, local_pose, global_pose});
-    data_.submap_data.at(submap_id).node_ids.emplace(node_id);
+    for (size_t i = 0; i < insertion_submaps.size(); ++i) {
-    const transform::Rigid3d constraint_transform =
+      const SubmapId submap_id = submap_ids[i];
-        insertion_submaps[i]->local_pose().inverse() * local_pose;
+      // Even if this was the last node added to 'submap_id', the submap will
-    data_.constraints.push_back(
+      // only be marked as finished in 'data_.submap_data' further below.
-        Constraint{submap_id,
+      CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive);
-                   node_id,
+      data_.submap_data.at(submap_id).node_ids.emplace(node_id);
-                   {constraint_transform, options_.matcher_translation_weight(),
+      const transform::Rigid3d constraint_transform =
-                    options_.matcher_rotation_weight()},
+          insertion_submaps[i]->local_pose().inverse() * local_pose;
-                   Constraint::INTRA_SUBMAP});
+      data_.constraints.push_back(Constraint{
-  }
+          submap_id,
-
+          node_id,
-  // TODO(gaschler): Consider not searching for constraints against trajectories
+          {constraint_transform, options_.matcher_translation_weight(),
-  // scheduled for deletion.
+           options_.matcher_rotation_weight()},
-  for (const auto& submap_id_data : data_.submap_data) {
+          Constraint::INTRA_SUBMAP});
-    if (submap_id_data.data.state == SubmapState::kFinished) {
+    }
-      CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
+    // TODO(gaschler): Consider not searching for constraints against
-      ComputeConstraint(node_id, submap_id_data.id);
+    // trajectories scheduled for deletion.
+    // TODO(danielsievers): Add a member variable and avoid having to copy
+    // them out here.
+    for (const auto& submap_id_data : data_.submap_data) {
+      if (submap_id_data.data.state == SubmapState::kFinished) {
+        CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
+        finished_submap_ids.emplace_back(submap_id_data.id);
+      }
+    }
+    if (newly_finished_submap) {
+      const SubmapId newly_finished_submap_id = submap_ids.front();
+      InternalSubmapData& finished_submap_data =
+          data_.submap_data.at(newly_finished_submap_id);
+      CHECK(finished_submap_data.state == SubmapState::kActive);
+      finished_submap_data.state = SubmapState::kFinished;
+      newly_finished_submap_node_ids = finished_submap_data.node_ids;
     }
   }
 
+  for (const auto& submap_id : finished_submap_ids) {
+    ComputeConstraint(node_id, submap_id);
+  }
+
   if (newly_finished_submap) {
-    const SubmapId finished_submap_id = submap_ids.front();
+    const SubmapId newly_finished_submap_id = submap_ids.front();
-    InternalSubmapData& finished_submap_data =
-        data_.submap_data.at(finished_submap_id);
-    CHECK(finished_submap_data.state == SubmapState::kActive);
-    finished_submap_data.state = SubmapState::kFinished;
     // We have a new completed submap, so we look into adding constraints for
     // old nodes.
-    ComputeConstraintsForOldNodes(finished_submap_id);
+    for (const auto& node_id_data : optimization_problem_->node_data()) {
+      const NodeId& node_id = node_id_data.id;
+      if (newly_finished_submap_node_ids.count(node_id) == 0) {
+        ComputeConstraint(node_id, newly_finished_submap_id);
+      }
+    }
   }
   constraint_builder_.NotifyEndOfNode();
   ++num_nodes_since_last_loop_closure_;

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -189,11 +189,7 @@ class PoseGraph3D : public PoseGraph {
 
   // Computes constraints for a node and submap pair.
   void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id)
-      REQUIRES(mutex_);
+      EXCLUDES(mutex_);
-
-  // Adds constraints for older nodes whenever a new submap is finished.
-  void ComputeConstraintsForOldNodes(const SubmapId& submap_id)
-      REQUIRES(mutex_);
 
   // Deletes trajectories waiting for deletion. Must not be called during
   // constraint search.