diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc
index 3869617..883d0f8 100644
--- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc
+++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc
@@ -178,7 +178,7 @@ std::vector<SubmapId> FindSubmapIdsToTrim(
 }  // namespace
 
 void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
-  const auto submap_data = pose_graph->GetAllSubmapData();
+  const auto submap_data = pose_graph->GetOptimizedSubmapData();
   if (submap_data.size() - current_submap_count_ <= min_added_submaps_count_) {
     return;
   }
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc
index 74ecf75..71b85ec 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.cc
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc
@@ -797,8 +797,20 @@ int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
 }
 
 MapById<SubmapId, PoseGraphInterface::SubmapData>
-PoseGraph2D::TrimmingHandle::GetAllSubmapData() const {
-  return parent_->GetSubmapDataUnderLock();
+PoseGraph2D::TrimmingHandle::GetOptimizedSubmapData() const {
+  MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
+  for (const auto& submap_id_data : parent_->submap_data_) {
+    if (submap_id_data.data.state != SubmapState::kFinished ||
+        !parent_->global_submap_poses_.Contains(submap_id_data.id)) {
+      continue;
+    }
+    submaps.Insert(submap_id_data.id,
+                   SubmapData{submap_id_data.data.submap,
+                              transform::Embed3D(parent_->global_submap_poses_
+                                                     .at(submap_id_data.id)
+                                                     .global_pose)});
+  }
+  return submaps;
 }
 
 std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h
index 971683e..5114300 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.h
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.h
@@ -281,7 +281,7 @@ class PoseGraph2D : public PoseGraph {
 
     int num_submaps(int trajectory_id) const override;
     std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
-    MapById<SubmapId, SubmapData> GetAllSubmapData() const override
+    MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
         REQUIRES(parent_->mutex_);
     const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
         REQUIRES(parent_->mutex_);
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc
index ce9eaa9..4f96624 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.cc
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc
@@ -835,8 +835,20 @@ std::vector<SubmapId> PoseGraph3D::TrimmingHandle::GetSubmapIds(
   return submap_ids;
 }
 MapById<SubmapId, PoseGraphInterface::SubmapData>
-PoseGraph3D::TrimmingHandle::GetAllSubmapData() const {
-  return parent_->GetSubmapDataUnderLock();
+PoseGraph3D::TrimmingHandle::GetOptimizedSubmapData() const {
+  MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
+  for (const auto& submap_id_data : parent_->submap_data_) {
+    if (submap_id_data.data.state != SubmapState::kFinished ||
+        !parent_->global_submap_poses_.Contains(submap_id_data.id)) {
+      continue;
+    }
+    submaps.Insert(
+        submap_id_data.id,
+        SubmapData{
+            submap_id_data.data.submap,
+            parent_->global_submap_poses_.at(submap_id_data.id).global_pose});
+  }
+  return submaps;
 }
 
 const MapById<NodeId, TrajectoryNode>&
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h
index cf51dd9..4411010 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.h
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.h
@@ -285,7 +285,7 @@ class PoseGraph3D : public PoseGraph {
 
     int num_submaps(int trajectory_id) const override;
     std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
-    MapById<SubmapId, SubmapData> GetAllSubmapData() const override
+    MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
         REQUIRES(parent_->mutex_);
     const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
         REQUIRES(parent_->mutex_);
diff --git a/cartographer/mapping/internal/testing/fake_trimmable.h b/cartographer/mapping/internal/testing/fake_trimmable.h
index cb5f720..29113d3 100644
--- a/cartographer/mapping/internal/testing/fake_trimmable.h
+++ b/cartographer/mapping/internal/testing/fake_trimmable.h
@@ -58,7 +58,7 @@ class FakeTrimmable : public Trimmable {
     return &submap_data_;
   }
 
-  MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
+  MapById<SubmapId, PoseGraphInterface::SubmapData> GetOptimizedSubmapData()
       const override {
     return submap_data_;
   }
diff --git a/cartographer/mapping/pose_graph_trimmer.h b/cartographer/mapping/pose_graph_trimmer.h
index 748026d..3ac1879 100644
--- a/cartographer/mapping/pose_graph_trimmer.h
+++ b/cartographer/mapping/pose_graph_trimmer.h
@@ -32,8 +32,9 @@ class Trimmable {
   virtual int num_submaps(int trajectory_id) const = 0;
 
   virtual std::vector<SubmapId> GetSubmapIds(int trajectory_id) const = 0;
-  virtual MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
-      const = 0;
+  // Returns finished submaps with optimized poses only.
+  virtual MapById<SubmapId, PoseGraphInterface::SubmapData>
+  GetOptimizedSubmapData() const = 0;
   virtual const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const = 0;
   virtual const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
       const = 0;