Fine-grained locking for PoseGraph2D constraints. (#1327)

This is #1279 for 2D.

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -34,6 +34,7 @@
 #include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h"
 #include "cartographer/sensor/compressed_point_cloud.h"
 #include "cartographer/sensor/internal/voxel_filter.h"
+#include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -240,6 +241,12 @@ void PoseGraph2D::AddLandmarkData(int trajectory_id,
 
 void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
                                     const SubmapId& submap_id) {
+  bool maybe_add_local_constraint = false;
+  bool maybe_add_global_constraint = false;
+  const TrajectoryNode::Data* constant_data;
+  const Submap2D* submap;
+  {
+    common::MutexLocker locker(&mutex_);
     CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
 
     const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
@@ -255,33 +262,26 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
       // 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()) {
+      maybe_add_global_constraint = true;
+    }
+    constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
+    submap = static_cast<const Submap2D*>(
+        data_.submap_data.at(submap_id).submap.get());
+  }
+
+  if (maybe_add_local_constraint) {
     const transform::Rigid2d initial_relative_pose =
         optimization_problem_->submap_data()
             .at(submap_id)
             .global_pose.inverse() *
         optimization_problem_->node_data().at(node_id).global_pose_2d;
     constraint_builder_.MaybeAddConstraint(
-        submap_id,
+        submap_id, submap, node_id, constant_data, initial_relative_pose);
-        static_cast<const Submap2D*>(
+  } else if (maybe_add_global_constraint) {
-            data_.submap_data.at(submap_id).submap.get()),
+    constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id,
-        node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
+                                                 constant_data);
-        initial_relative_pose);
-  } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
-    constraint_builder_.MaybeAddGlobalConstraint(
-        submap_id,
-        static_cast<const Submap2D*>(
-            data_.submap_data.at(submap_id).submap.get()),
-        node_id, data_.trajectory_nodes.at(node_id).constant_data.get());
-  }
-}
-
-void PoseGraph2D::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);
-    }
   }
 }
 
@@ -289,15 +289,21 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
     const NodeId& node_id,
     std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
     const bool newly_finished_submap) {
+  std::vector<SubmapId> submap_ids;
+  std::vector<SubmapId> finished_submap_ids;
+  std::set<NodeId> newly_finished_submap_node_ids;
+  {
     common::MutexLocker locker(&mutex_);
-  const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data;
+    const auto& constant_data =
-  const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
+        data_.trajectory_nodes.at(node_id).constant_data;
+    submap_ids = InitializeGlobalSubmapPoses(
         node_id.trajectory_id, constant_data->time, insertion_submaps);
     CHECK_EQ(submap_ids.size(), insertion_submaps.size());
     const SubmapId matching_id = submap_ids.front();
-  const transform::Rigid2d local_pose_2d = transform::Project2D(
+    const transform::Rigid2d local_pose_2d =
-      constant_data->local_pose *
+        transform::Project2D(constant_data->local_pose *
-      transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
+                             transform::Rigid3d::Rotation(
+                                 constant_data->gravity_alignment.inverse()));
     const transform::Rigid2d global_pose_2d =
         optimization_problem_->submap_data().at(matching_id).global_pose *
         constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
@@ -325,24 +331,40 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
                      Constraint::INTRA_SUBMAP});
     }
 
-  // TODO(gaschler): Consider not searching for constraints against trajectories
+    // TODO(gaschler): Consider not searching for constraints against
-  // scheduled for deletion.
+    // 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);
-      ComputeConstraint(node_id, submap_id_data.id);
+        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/2d/pose_graph_2d.h

@@ -188,11 +188,7 @@ class PoseGraph2D : 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.