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();
-    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();
-    }
   }
 }
 
+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
+    CHECK(!run_loop_closure_);
-    // ComputeConstraintsForNode and call from here
+    DispatchOptimization();
   });
 }
 
@@ -505,14 +508,21 @@ void PoseGraph::AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
 }
 
 void PoseGraph::RunFinalOptimization() {
+  {
+    common::MutexLocker locker(&mutex_);
+    AddWorkItem([this]() REQUIRES(mutex_) {
+      optimization_problem_.SetMaxNumIterations(
+          options_.max_num_final_iterations());
+      DispatchOptimization();
+    });
+    AddWorkItem([this]() REQUIRES(mutex_) {
+      optimization_problem_.SetMaxNumIterations(
+          options_.optimization_problem_options()
+              .ceres_solver_options()
+              .max_num_iterations());
+    });
+  }
   WaitForAllComputations();
-  optimization_problem_.SetMaxNumIterations(
-      options_.max_num_final_iterations());
-  RunOptimization();
-  optimization_problem_.SetMaxNumIterations(
-      options_.optimization_problem_options()
-          .ceres_solver_options()
-          .max_num_iterations());
 }
 
 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,15 +286,19 @@ 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();
-    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()>>>();
+void PoseGraph::DispatchOptimization() {
-      HandleWorkQueue();
+  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();
   }
 }
 
@@ -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
+    CHECK(!run_loop_closure_);
-    // ComputeConstraintsForNode and call from here
+    DispatchOptimization();
   });
 }
 
@@ -507,14 +511,21 @@ void PoseGraph::AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
 }
 
 void PoseGraph::RunFinalOptimization() {
+  {
+    common::MutexLocker locker(&mutex_);
+    AddWorkItem([this]() REQUIRES(mutex_) {
+      optimization_problem_.SetMaxNumIterations(
+          options_.max_num_final_iterations());
+      DispatchOptimization();
+    });
+    AddWorkItem([this]() REQUIRES(mutex_) {
+      optimization_problem_.SetMaxNumIterations(
+          options_.optimization_problem_options()
+              .ceres_solver_options()
+              .max_num_iterations());
+    });
+  }
   WaitForAllComputations();
-  optimization_problem_.SetMaxNumIterations(
-      options_.max_num_final_iterations());
-  RunOptimization();
-  optimization_problem_.SetMaxNumIterations(
-      options_.optimization_problem_options()
-          .ceres_solver_options()
-          .max_num_iterations());
 }
 
 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_);