Take PoseGraph2D mutex inside instead of outside work item. (#1310)

This is #1264 for 2D.
2018-07-20 13:31:18 +02:00 · 2018-07-20 13:31:18 +02:00 · 5077224f8e
parent 665b95d5c6
commit 5077224f8e
2 changed files with 159 additions and 108 deletions
--- a/cartographer/mapping/internal/2d/pose_graph_2d.cc
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc
@ -106,13 +106,11 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
  return {front_submap_id, last_submap_id};
 }

-NodeId PoseGraph2D::AddNode(
+NodeId PoseGraph2D::AppendNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
-    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
-  const transform::Rigid3d optimized_pose(
-      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
-
+    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
+    const transform::Rigid3d& optimized_pose) {
  common::MutexLocker locker(&mutex_);
  AddTrajectoryIfNeeded(trajectory_id);
  if (!CanAddWorkItemModifying(trajectory_id)) {
@ -121,7 +119,6 @@ NodeId PoseGraph2D::AddNode(
  const NodeId node_id = data_.trajectory_nodes.Append(
      trajectory_id, TrajectoryNode{constant_data, optimized_pose});
  ++data_.num_trajectory_nodes;
-
  // Test if the 'insertion_submap.back()' is one we never saw before.
  if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
@ -133,11 +130,22 @@ NodeId PoseGraph2D::AddNode(
    data_.submap_data.at(submap_id).submap = insertion_submaps.back();
    LOG(INFO) << "Inserted submap " << submap_id << ".";
  }
+  return node_id;
+}

+NodeId PoseGraph2D::AddNode(
+    std::shared_ptr<const TrajectoryNode::Data> constant_data,
+    const int trajectory_id,
+    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
+  const transform::Rigid3d optimized_pose(
+      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
+
+  const NodeId node_id = AppendNode(constant_data, trajectory_id,
+                                    insertion_submaps, optimized_pose);
  // We have to check this here, because it might have changed by the time we
  // execute the lambda.
  const bool newly_finished_submap = insertion_submaps.front()->finished();
-  AddWorkItem([=]() REQUIRES(mutex_) {
+  AddWorkItem([=]() EXCLUDES(mutex_) {
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });
@ -146,19 +154,25 @@ NodeId PoseGraph2D::AddNode(

 void PoseGraph2D::AddWorkItem(
    const std::function<WorkItem::Result()>& work_item) {
-  if (work_queue_ == nullptr) {
-    if (work_item() == WorkItem::Result::kRunOptimization) {
-      work_queue_ = common::make_unique<WorkQueue>();
-      constraint_builder_.WhenDone(
-          [this](const constraints::ConstraintBuilder2D::Result& result) {
-            HandleWorkQueue(result);
-          });
+  {
+    common::MutexLocker locker(&mutex_);
+    if (work_queue_ != nullptr) {
+      const auto now = std::chrono::steady_clock::now();
+      work_queue_->push_back({now, work_item});
+      kWorkQueueDelayMetric->Set(
+          common::ToSeconds(now - work_queue_->front().time));
+      return;
    }
-  } else {
-    const auto now = std::chrono::steady_clock::now();
-    work_queue_->push_back({now, work_item});
-    kWorkQueueDelayMetric->Set(
-        common::ToSeconds(now - work_queue_->front().time));
+  }
+  if (work_item() == WorkItem::Result::kRunOptimization) {
+    {
+      common::MutexLocker locker(&mutex_);
+      work_queue_ = common::make_unique<WorkQueue>();
+    }
+    constraint_builder_.WhenDone(
+        [this](const constraints::ConstraintBuilder2D::Result& result) {
+          HandleWorkQueue(result);
+        });
  }
 }

@ -181,20 +195,22 @@ void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {

 void PoseGraph2D::AddImuData(const int trajectory_id,
                             const sensor::ImuData& imu_data) {
-  common::MutexLocker locker(&mutex_);
-  if (!CanAddWorkItemModifying(trajectory_id)) return;
-  AddWorkItem([=]() REQUIRES(mutex_) {
-    optimization_problem_->AddImuData(trajectory_id, imu_data);
+  AddWorkItem([=]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
+    if (CanAddWorkItemModifying(trajectory_id)) {
+      optimization_problem_->AddImuData(trajectory_id, imu_data);
+    }
    return WorkItem::Result::kDoNotRunOptimization;
  });
 }

 void PoseGraph2D::AddOdometryData(const int trajectory_id,
                                  const sensor::OdometryData& odometry_data) {
-  common::MutexLocker locker(&mutex_);
-  if (!CanAddWorkItemModifying(trajectory_id)) return;
-  AddWorkItem([=]() REQUIRES(mutex_) {
-    optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
+  AddWorkItem([=]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
+    if (CanAddWorkItemModifying(trajectory_id)) {
+      optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
+    }
    return WorkItem::Result::kDoNotRunOptimization;
  });
 }
@ -207,15 +223,16 @@ void PoseGraph2D::AddFixedFramePoseData(

 void PoseGraph2D::AddLandmarkData(int trajectory_id,
                                  const sensor::LandmarkData& landmark_data) {
-  common::MutexLocker locker(&mutex_);
-  if (!CanAddWorkItemModifying(trajectory_id)) return;
-  AddWorkItem([=]() REQUIRES(mutex_) {
-    for (const auto& observation : landmark_data.landmark_observations) {
-      data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
-          LandmarkNode::LandmarkObservation{
-              trajectory_id, landmark_data.time,
-              observation.landmark_to_tracking_transform,
-              observation.translation_weight, observation.rotation_weight});
+  AddWorkItem([=]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
+    if (CanAddWorkItemModifying(trajectory_id)) {
+      for (const auto& observation : landmark_data.landmark_observations) {
+        data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
+            PoseGraphInterface::LandmarkNode::LandmarkObservation{
+                trajectory_id, landmark_data.time,
+                observation.landmark_to_tracking_transform,
+                observation.translation_weight, observation.rotation_weight});
+      }
    }
    return WorkItem::Result::kDoNotRunOptimization;
  });
@ -272,6 +289,7 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap) {
+  common::MutexLocker locker(&mutex_);
  const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data;
  const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
      node_id.trajectory_id, constant_data->time, insertion_submaps);
@ -409,34 +427,43 @@ void PoseGraph2D::HandleWorkQueue(
        trajectory_id_to_last_optimized_node_id);
  }

-  common::MutexLocker locker(&mutex_);
-  for (const Constraint& constraint : result) {
-    UpdateTrajectoryConnectivity(constraint);
-  }
-  DeleteTrajectoriesIfNeeded();
-  TrimmingHandle trimming_handle(this);
-  for (auto& trimmer : trimmers_) {
-    trimmer->Trim(&trimming_handle);
-  }
-  trimmers_.erase(
-      std::remove_if(trimmers_.begin(), trimmers_.end(),
-                     [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
-                       return trimmer->IsFinished();
-                     }),
-      trimmers_.end());
+  {
+    common::MutexLocker locker(&mutex_);
+    for (const Constraint& constraint : result) {
+      UpdateTrajectoryConnectivity(constraint);
+    }
+    DeleteTrajectoriesIfNeeded();
+    TrimmingHandle trimming_handle(this);
+    for (auto& trimmer : trimmers_) {
+      trimmer->Trim(&trimming_handle);
+    }
+    trimmers_.erase(
+        std::remove_if(trimmers_.begin(), trimmers_.end(),
+                       [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
+                         return trimmer->IsFinished();
+                       }),
+        trimmers_.end());

-  num_nodes_since_last_loop_closure_ = 0;
+    num_nodes_since_last_loop_closure_ = 0;
+  }
+
+  size_t work_queue_size;
  bool process_work_queue = true;
  while (process_work_queue) {
-    if (work_queue_->empty()) {
-      work_queue_.reset();
-      return;
+    std::function<WorkItem::Result()> work_item;
+    {
+      common::MutexLocker locker(&mutex_);
+      if (work_queue_->empty()) {
+        work_queue_.reset();
+        return;
+      }
+      work_item = work_queue_->front().task;
+      work_queue_->pop_front();
+      work_queue_size = work_queue_->size();
    }
-    process_work_queue =
-        work_queue_->front().task() == WorkItem::Result::kDoNotRunOptimization;
-    work_queue_->pop_front();
+    process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
  }
-  LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
+  LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
  // We have to optimize again.
  constraint_builder_.WhenDone(
      [this](const constraints::ConstraintBuilder2D::Result& result) {
@ -482,16 +509,19 @@ void PoseGraph2D::WaitForAllComputations() {
 }

 void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
-  common::MutexLocker locker(&mutex_);
-  auto it = data_.trajectories_state.find(trajectory_id);
-  if (it == data_.trajectories_state.end()) {
-    LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: "
-                 << trajectory_id;
-    return;
+  {
+    common::MutexLocker locker(&mutex_);
+    auto it = data_.trajectories_state.find(trajectory_id);
+    if (it == data_.trajectories_state.end()) {
+      LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: "
+                   << trajectory_id;
+      return;
+    }
+    it->second.deletion_state =
+        InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
  }
-  it->second.deletion_state =
-      InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
-  AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
+  AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
    CHECK(data_.trajectories_state.at(trajectory_id).state !=
          TrajectoryState::ACTIVE);
    CHECK(data_.trajectories_state.at(trajectory_id).state !=
@ -505,8 +535,8 @@ void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
 }

 void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
-  common::MutexLocker locker(&mutex_);
-  AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
+  AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
    CHECK(!IsTrajectoryFinished(trajectory_id));
    data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;

@ -524,9 +554,12 @@ bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const {
 }

 void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
-  common::MutexLocker locker(&mutex_);
-  data_.trajectory_connectivity_state.Add(trajectory_id);
-  AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
+  {
+    common::MutexLocker locker(&mutex_);
+    data_.trajectory_connectivity_state.Add(trajectory_id);
+  }
+  AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
    CHECK(!IsTrajectoryFrozen(trajectory_id));
    data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
    return WorkItem::Result::kDoNotRunOptimization;
@ -548,21 +581,25 @@ void PoseGraph2D::AddSubmapFromProto(
  const SubmapId submap_id = {submap.submap_id().trajectory_id(),
                              submap.submap_id().submap_index()};

-  common::MutexLocker locker(&mutex_);
-  std::shared_ptr<const Submap2D> submap_ptr =
-      std::make_shared<const Submap2D>(submap.submap_2d(), &conversion_tables_);
  const transform::Rigid2d global_submap_pose_2d =
      transform::Project2D(global_submap_pose);
-  AddTrajectoryIfNeeded(submap_id.trajectory_id);
-  if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
-  data_.submap_data.Insert(submap_id, InternalSubmapData());
-  data_.submap_data.at(submap_id).submap = submap_ptr;
-  // Immediately show the submap at the 'global_submap_pose'.
-  data_.global_submap_poses_2d.Insert(
-      submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
-  bool finished = submap.submap_2d().finished();
+  {
+    common::MutexLocker locker(&mutex_);
+    const std::shared_ptr<const Submap2D> submap_ptr =
+        std::make_shared<const Submap2D>(submap.submap_2d(),
+                                         &conversion_tables_);
+    AddTrajectoryIfNeeded(submap_id.trajectory_id);
+    if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
+    data_.submap_data.Insert(submap_id, InternalSubmapData());
+    data_.submap_data.at(submap_id).submap = submap_ptr;
+    // Immediately show the submap at the 'global_submap_pose'.
+    data_.global_submap_poses_2d.Insert(
+        submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
+  }
+  const bool finished = submap.submap_2d().finished();
  AddWorkItem(
-      [this, submap_id, global_submap_pose_2d, finished]() REQUIRES(mutex_) {
+      [this, submap_id, global_submap_pose_2d, finished]() EXCLUDES(mutex_) {
+        common::MutexLocker locker(&mutex_);
        data_.submap_data.at(submap_id).state =
            finished ? SubmapState::kFinished : SubmapState::kActive;
        optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
@ -577,13 +614,16 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
  std::shared_ptr<const TrajectoryNode::Data> constant_data =
      std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data()));

-  common::MutexLocker locker(&mutex_);
-  AddTrajectoryIfNeeded(node_id.trajectory_id);
-  if (!CanAddWorkItemModifying(node_id.trajectory_id)) return;
-  data_.trajectory_nodes.Insert(node_id,
-                                TrajectoryNode{constant_data, global_pose});
+  {
+    common::MutexLocker locker(&mutex_);
+    AddTrajectoryIfNeeded(node_id.trajectory_id);
+    if (!CanAddWorkItemModifying(node_id.trajectory_id)) return;
+    data_.trajectory_nodes.Insert(node_id,
+                                  TrajectoryNode{constant_data, global_pose});
+  }

-  AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
+  AddWorkItem([this, node_id, global_pose]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
    const auto& constant_data =
        data_.trajectory_nodes.at(node_id).constant_data;
    const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
@ -607,18 +647,19 @@ void PoseGraph2D::SetTrajectoryDataFromProto(

 void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id,
                                  const SubmapId& submap_id) {
-  common::MutexLocker locker(&mutex_);
-  if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
-  AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
-    data_.submap_data.at(submap_id).node_ids.insert(node_id);
+  AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
+    if (CanAddWorkItemModifying(submap_id.trajectory_id)) {
+      data_.submap_data.at(submap_id).node_ids.insert(node_id);
+    }
    return WorkItem::Result::kDoNotRunOptimization;
  });
 }

 void PoseGraph2D::AddSerializedConstraints(
    const std::vector<Constraint>& constraints) {
-  common::MutexLocker locker(&mutex_);
-  AddWorkItem([this, constraints]() REQUIRES(mutex_) {
+  AddWorkItem([this, constraints]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
    for (const auto& constraint : constraints) {
      CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
      CHECK(data_.submap_data.Contains(constraint.submap_id));
@ -650,10 +691,10 @@ void PoseGraph2D::AddSerializedConstraints(
 }

 void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
-  common::MutexLocker locker(&mutex_);
  // C++11 does not allow us to move a unique_ptr into a lambda.
  PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
-  AddWorkItem([this, trimmer_ptr]() REQUIRES(mutex_) {
+  AddWorkItem([this, trimmer_ptr]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
    trimmers_.emplace_back(trimmer_ptr);
    return WorkItem::Result::kDoNotRunOptimization;
  });
@ -661,13 +702,14 @@ void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {

 void PoseGraph2D::RunFinalOptimization() {
  {
-    common::MutexLocker locker(&mutex_);
-    AddWorkItem([this]() REQUIRES(mutex_) {
+    AddWorkItem([this]() EXCLUDES(mutex_) {
+      common::MutexLocker locker(&mutex_);
      optimization_problem_->SetMaxNumIterations(
          options_.max_num_final_iterations());
      return WorkItem::Result::kRunOptimization;
    });
-    AddWorkItem([this]() REQUIRES(mutex_) {
+    AddWorkItem([this]() EXCLUDES(mutex_) {
+      common::MutexLocker locker(&mutex_);
      optimization_problem_->SetMaxNumIterations(
          options_.optimization_problem_options()
              .ceres_solver_options()
@ -807,8 +849,8 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()

 void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id,
                                  const transform::Rigid3d& global_pose) {
-  common::MutexLocker locker(&mutex_);
-  AddWorkItem([=]() REQUIRES(mutex_) {
+  AddWorkItem([=]() EXCLUDES(mutex_) {
+    common::MutexLocker locker(&mutex_);
    data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
    return WorkItem::Result::kDoNotRunOptimization;
  });
--- a/cartographer/mapping/internal/2d/pose_graph_2d.h
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.h
@ -40,6 +40,7 @@
 #include "cartographer/mapping/pose_graph.h"
 #include "cartographer/mapping/pose_graph_data.h"
 #include "cartographer/mapping/pose_graph_trimmer.h"
+#include "cartographer/mapping/value_conversion_tables.h"
 #include "cartographer/metrics/family_factory.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/landmark_data.h"
@ -159,11 +160,19 @@ class PoseGraph2D : public PoseGraph {

  // Handles a new work item.
  void AddWorkItem(const std::function<WorkItem::Result()>& work_item)
-      REQUIRES(mutex_);
+      EXCLUDES(mutex_);

  // Adds connectivity and sampler for a trajectory if it does not exist.
  void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_);

+  // Appends the new node and submap (if needed) to the internal data
+  // structures.
+  NodeId AppendNode(
+      std::shared_ptr<const TrajectoryNode::Data> constant_data,
+      int trajectory_id,
+      const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
+      const transform::Rigid3d& optimized_pose) EXCLUDES(mutex_);
+
  // Grows the optimization problem to have an entry for every element of
  // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
  std::vector<SubmapId> InitializeGlobalSubmapPoses(
@ -175,7 +184,7 @@ class PoseGraph2D : public PoseGraph {
  WorkItem::Result ComputeConstraintsForNode(
      const NodeId& node_id,
      std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
-      bool newly_finished_submap) REQUIRES(mutex_);
+      bool newly_finished_submap) EXCLUDES(mutex_);

  // Computes constraints for a node and submap pair.
  void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id)
@ -191,7 +200,7 @@ class PoseGraph2D : public PoseGraph {

  // Runs the optimization, executes the trimmers and processes the work queue.
  void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)
-      REQUIRES(mutex_);
+      EXCLUDES(mutex_);

  // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
  // all computations have finished.
@ -237,7 +246,7 @@ class PoseGraph2D : public PoseGraph {

  // Current optimization problem.
  std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
-  constraints::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_);
+  constraints::ConstraintBuilder2D constraint_builder_;

  // List of all trimmers to consult when optimizations finish.
  std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);