Refactor calling optimization into DispatchOptimization. (#729)

I noticed that @jihoonl opened the PR #726 which performs a similar thing. As discussed in googlecartographer/cartographer_ros#613 (@cschuet  has already taken a look), I pulled this out of #481 (a really old PR whose merging has been postponed), which is an example where re-running optimization is triggered from elsewhere as well (besides from `ComputeConstraintsForNode`). This refactoring makes libcartographer friendlier for use cases such as that one.

An important detail is that I have changed the condition in `WaitForAllComputations` to also check if the work queue is empty. If there are other things on the worker queue besides `ComputeConstraintsForNode`, currently we will wrongfully conclude that all computations are done. (This detail was merged in #754, so it's no longer in the diff of this PR).

Also missing is the same thing for 3D. I can add that when we settle on this.

Also, I suggest that `run_loop_closure` gets renamed to `run_optimization`.

cartographer/mapping_2d/pose_graph.cc

@@ -269,18 +269,21 @@ void PoseGraph::ComputeConstraintsForNode(
   }
   constraint_builder_.NotifyEndOfNode();
   ++num_nodes_since_last_loop_closure_;
+  CHECK(!run_loop_closure_);
   if (options_.optimize_every_n_nodes() > 0 &&
       num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
-    CHECK(!run_loop_closure_);
+    DispatchOptimization();
+  }
+}
+
+void PoseGraph::DispatchOptimization() {
   run_loop_closure_ = true;
   // If there is a 'work_queue_' already, some other thread will take care.
   if (work_queue_ == nullptr) {
     work_queue_ = common::make_unique<std::deque<std::function<void()>>>();
     HandleWorkQueue();
   }
-  }
 }
-
 common::Time PoseGraph::GetLatestNodeTime(
     const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const {
   common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
@@ -384,8 +387,8 @@ void PoseGraph::FinishTrajectory(const int trajectory_id) {
     for (const auto& submap : submap_data_.trajectory(trajectory_id)) {
       submap_data_.at(submap.id).state = SubmapState::kFinished;
     }
-    // TODO(jihoonl): Refactor HandleWorkQueue() logic from
-    // ComputeConstraintsForNode and call from here
+    CHECK(!run_loop_closure_);
+    DispatchOptimization();
   });
 }
 
@@ -505,14 +508,21 @@ void PoseGraph::AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
 }
 
 void PoseGraph::RunFinalOptimization() {
-  WaitForAllComputations();
+  {
+    common::MutexLocker locker(&mutex_);
+    AddWorkItem([this]() REQUIRES(mutex_) {
       optimization_problem_.SetMaxNumIterations(
           options_.max_num_final_iterations());
-  RunOptimization();
+      DispatchOptimization();
+    });
+    AddWorkItem([this]() REQUIRES(mutex_) {
       optimization_problem_.SetMaxNumIterations(
           options_.optimization_problem_options()
               .ceres_solver_options()
               .max_num_iterations());
+    });
+  }
+  WaitForAllComputations();
 }
 
 void PoseGraph::RunOptimization() {

cartographer/mapping_2d/pose_graph.h

@@ -215,6 +215,9 @@ class PoseGraph : public mapping::PoseGraph {
   // Whether the optimization has to be run before more data is added.
   bool run_loop_closure_ GUARDED_BY(mutex_) = false;
 
+  // Schedules optimization (i.e. loop closure) to run.
+  void DispatchOptimization() REQUIRES(mutex_);
+
   // Current optimization problem.
   pose_graph::OptimizationProblem optimization_problem_;
   pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);

cartographer/mapping_3d/pose_graph.cc

@@ -286,16 +286,20 @@ void PoseGraph::ComputeConstraintsForNode(
   }
   constraint_builder_.NotifyEndOfNode();
   ++num_nodes_since_last_loop_closure_;
+  CHECK(!run_loop_closure_);
   if (options_.optimize_every_n_nodes() > 0 &&
       num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
-    CHECK(!run_loop_closure_);
+    DispatchOptimization();
+  }
+}
+
+void PoseGraph::DispatchOptimization() {
   run_loop_closure_ = true;
   // If there is a 'work_queue_' already, some other thread will take care.
   if (work_queue_ == nullptr) {
     work_queue_ = common::make_unique<std::deque<std::function<void()>>>();
     HandleWorkQueue();
   }
-  }
 }
 
 common::Time PoseGraph::GetLatestNodeTime(
@@ -401,8 +405,8 @@ void PoseGraph::FinishTrajectory(const int trajectory_id) {
     for (const auto& submap : submap_data_.trajectory(trajectory_id)) {
       submap_data_.at(submap.id).state = SubmapState::kFinished;
     }
-    // TODO(jihoonl): Refactor HandleWorkQueue() logic from
-    // ComputeConstraintsForNode and call from here
+    CHECK(!run_loop_closure_);
+    DispatchOptimization();
   });
 }
 
@@ -507,14 +511,21 @@ void PoseGraph::AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
 }
 
 void PoseGraph::RunFinalOptimization() {
-  WaitForAllComputations();
+  {
+    common::MutexLocker locker(&mutex_);
+    AddWorkItem([this]() REQUIRES(mutex_) {
       optimization_problem_.SetMaxNumIterations(
           options_.max_num_final_iterations());
-  RunOptimization();
+      DispatchOptimization();
+    });
+    AddWorkItem([this]() REQUIRES(mutex_) {
       optimization_problem_.SetMaxNumIterations(
           options_.optimization_problem_options()
               .ceres_solver_options()
               .max_num_iterations());
+    });
+  }
+  WaitForAllComputations();
 }
 
 void PoseGraph::LogResidualHistograms() {

cartographer/mapping_3d/pose_graph.h

@@ -219,6 +219,9 @@ class PoseGraph : public mapping::PoseGraph {
   // Whether the optimization has to be run before more data is added.
   bool run_loop_closure_ GUARDED_BY(mutex_) = false;
 
+  // Schedules optimization (i.e. loop closure) to run.
+  void DispatchOptimization() REQUIRES(mutex_);
+
   // Current optimization problem.
   pose_graph::OptimizationProblem optimization_problem_;
   pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);