diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index f462557..34c8a7c 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -305,6 +305,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
   for (const Constraint& constraint : result) {
     CHECK_EQ(constraint.tag,
              mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
+    CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
     common::Time time =
         trajectory_nodes_.at(constraint.node_id).constant_data->time;
     const SubmapData& submap_data = submap_data_.at(constraint.submap_id);
@@ -328,9 +329,14 @@ void SparsePoseGraph::HandleWorkQueue() {
           constraints_.insert(constraints_.end(), result.begin(), result.end());
         }
         RunOptimization();
-        UpdateTrajectoryConnectivity(result);
 
         common::MutexLocker locker(&mutex_);
+        UpdateTrajectoryConnectivity(result);
+        TrimmingHandle trimming_handle(this);
+        for (auto& trimmer : trimmers_) {
+          trimmer->Trim(&trimming_handle);
+        }
+
         num_scans_since_last_loop_closure_ = 0;
         run_loop_closure_ = false;
         while (!run_loop_closure_) {
@@ -479,11 +485,6 @@ void SparsePoseGraph::RunOptimization() {
     }
   }
   optimized_submap_transforms_ = submap_data;
-
-  TrimmingHandle trimming_handle(this);
-  for (auto& trimmer : trimmers_) {
-    trimmer->Trim(&trimming_handle);
-  }
 }
 
 std::vector<std::vector<mapping::TrajectoryNode>>
diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h
index 0741fca..d055623 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.h
+++ b/cartographer/mapping_2d/sparse_pose_graph.h
@@ -155,7 +155,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Updates the trajectory connectivity structure with the new constraints.
   void UpdateTrajectoryConnectivity(
-      const sparse_pose_graph::ConstraintBuilder::Result& result);
+      const sparse_pose_graph::ConstraintBuilder::Result& result)
+      REQUIRES(mutex_);
 
   // Computes the local to global frame transform based on the given optimized
   // 'submap_transforms'.
diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 1a0e5f0..43775e7 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -325,6 +325,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
   for (const Constraint& constraint : result) {
     CHECK_EQ(constraint.tag,
              mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
+    CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
     common::Time time =
         trajectory_nodes_.at(constraint.node_id).constant_data->time;
     const SubmapData& submap_data = submap_data_.at(constraint.submap_id);
@@ -348,9 +349,14 @@ void SparsePoseGraph::HandleWorkQueue() {
           constraints_.insert(constraints_.end(), result.begin(), result.end());
         }
         RunOptimization();
-        UpdateTrajectoryConnectivity(result);
 
         common::MutexLocker locker(&mutex_);
+        UpdateTrajectoryConnectivity(result);
+        TrimmingHandle trimming_handle(this);
+        for (auto& trimmer : trimmers_) {
+          trimmer->Trim(&trimming_handle);
+        }
+
         num_scans_since_last_loop_closure_ = 0;
         run_loop_closure_ = false;
         while (!run_loop_closure_) {
@@ -526,11 +532,6 @@ void SparsePoseGraph::RunOptimization() {
   }
   optimized_submap_transforms_ = submap_data;
 
-  TrimmingHandle trimming_handle(this);
-  for (auto& trimmer : trimmers_) {
-    trimmer->Trim(&trimming_handle);
-  }
-
   // Log the histograms for the pose residuals.
   if (options_.log_residual_histograms()) {
     LogResidualHistograms();
diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h
index bc47807..a41b312 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.h
+++ b/cartographer/mapping_3d/sparse_pose_graph.h
@@ -170,7 +170,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Updates the trajectory connectivity structure with the new constraints.
   void UpdateTrajectoryConnectivity(
-      const sparse_pose_graph::ConstraintBuilder::Result& result);
+      const sparse_pose_graph::ConstraintBuilder::Result& result)
+      REQUIRES(mutex_);
 
   const mapping::proto::SparsePoseGraphOptions options_;
   common::Mutex mutex_;