diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
index 0d1f6ac..5073b75 100644
--- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
@@ -168,6 +168,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
     const int trajectory_id = node_it->id.trajectory_id;
     const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
+    if (frozen_trajectories.count(trajectory_id) != 0) {
+      node_it = trajectory_end;
+      continue;
+    }
 
     auto prev_node_it = node_it;
     for (++node_it; node_it != trajectory_end; ++node_it) {
diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
index 78e7e35..8b254a8 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
@@ -309,6 +309,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
     for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
       const int trajectory_id = node_it->id.trajectory_id;
       const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
+      if (frozen_trajectories.count(trajectory_id) != 0) {
+        node_it = trajectory_end;
+        continue;
+      }
 
       auto prev_node_it = node_it;
       for (++node_it; node_it != trajectory_end; ++node_it) {