diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc
index fdee1bf..5fff41e 100644
--- a/cartographer/mapping_3d/pose_graph.cc
+++ b/cartographer/mapping_3d/pose_graph.cc
@@ -368,8 +368,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;