diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h
index c1625de..f0626f1 100644
--- a/cartographer/mapping/pose_graph_interface.h
+++ b/cartographer/mapping/pose_graph_interface.h
@@ -60,7 +60,7 @@ class PoseGraphInterface {
   PoseGraphInterface(const PoseGraphInterface&) = delete;
   PoseGraphInterface& operator=(const PoseGraphInterface&) = delete;
 
-  // Computes optimized poses.
+  // Waits for all computations to finish and computes optimized poses.
   virtual void RunFinalOptimization() = 0;
 
   // Returns data for all submaps.
diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc
index 02dc11b..c3d9fb5 100644
--- a/cartographer/mapping_2d/pose_graph.cc
+++ b/cartographer/mapping_2d/pose_graph.cc
@@ -351,8 +351,9 @@ void PoseGraph::WaitForAllComputations() {
       constraint_builder_.GetNumFinishedNodes();
   while (!locker.AwaitWithTimeout(
       [this]() REQUIRES(mutex_) {
-        return constraint_builder_.GetNumFinishedNodes() ==
-               num_trajectory_nodes_;
+        return ((constraint_builder_.GetNumFinishedNodes() ==
+                 num_trajectory_nodes_) &&
+                !work_queue_);
       },
       common::FromSeconds(1.))) {
     std::ostringstream progress_info;