diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h
index b171123..2dccce5 100644
--- a/cartographer/mapping/trajectory_builder.h
+++ b/cartographer/mapping/trajectory_builder.h
@@ -54,7 +54,7 @@ class TrajectoryBuilder {
   };
 
   struct SubmapData {
-    const Submap* submap;
+    std::shared_ptr<const Submap> submap;
     transform::Rigid3d pose;
   };
 
diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc
index 21db9e4..06cbfda 100644
--- a/cartographer/mapping_2d/global_trajectory_builder.cc
+++ b/cartographer/mapping_2d/global_trajectory_builder.cc
@@ -48,16 +48,11 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
   if (insertion_result == nullptr) {
     return;
   }
-  const Submap* const finished_submap =
-      insertion_result->insertion_submaps.front()->finished()
-          ? insertion_result->insertion_submaps.front()
-          : nullptr;
   sparse_pose_graph_->AddScan(
       insertion_result->time, insertion_result->tracking_to_tracking_2d,
       insertion_result->range_data_in_tracking_2d,
       insertion_result->pose_estimate_2d, trajectory_id_,
-      insertion_result->matching_submap, insertion_result->insertion_submaps,
-      finished_submap);
+      std::move(insertion_result->insertion_submaps));
 }
 
 void GlobalTrajectoryBuilder::AddImuData(
diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc
index cf44097..e37005b 100644
--- a/cartographer/mapping_2d/local_trajectory_builder.cc
+++ b/cartographer/mapping_2d/local_trajectory_builder.cc
@@ -176,8 +176,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
     return nullptr;
   }
 
-  const Submap* const matching_submap = submaps_.Get(submaps_.matching_index());
-  std::vector<const Submap*> insertion_submaps;
+  std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   for (int insertion_index : submaps_.insertion_indices()) {
     insertion_submaps.push_back(submaps_.Get(insertion_index));
   }
@@ -186,7 +185,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
                          transform::Embed3D(pose_estimate_2d.cast<float>())));
 
   return common::make_unique<InsertionResult>(InsertionResult{
-      time, matching_submap, insertion_submaps, tracking_to_tracking_2d,
+      time, std::move(insertion_submaps), tracking_to_tracking_2d,
       range_data_in_tracking_2d, pose_estimate_2d});
 }
 
diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h
index 13440de..eefeff5 100644
--- a/cartographer/mapping_2d/local_trajectory_builder.h
+++ b/cartographer/mapping_2d/local_trajectory_builder.h
@@ -41,8 +41,7 @@ class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
     common::Time time;
-    const Submap* matching_submap;
-    std::vector<const Submap*> insertion_submaps;
+    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
     transform::Rigid3d tracking_to_tracking_2d;
     sensor::RangeData range_data_in_tracking_2d;
     transform::Rigid2d pose_estimate_2d;
diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index 79c3d4b..549b3f4 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -54,7 +54,7 @@ SparsePoseGraph::~SparsePoseGraph() {
 
 std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
     const int trajectory_id,
-    const std::vector<const Submap*>& insertion_submaps) {
+    const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   CHECK(!insertion_submaps.empty());
   const auto& submap_data = optimization_problem_.submap_data();
   if (insertion_submaps.size() == 1) {
@@ -98,9 +98,8 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 void SparsePoseGraph::AddScan(
     common::Time time, const transform::Rigid3d& tracking_to_pose,
     const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
-    const int trajectory_id, const Submap* const matching_submap,
-    const std::vector<const Submap*>& insertion_submaps,
-    const Submap* const finished_submap) {
+    const int trajectory_id,
+    const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
 
@@ -136,9 +135,12 @@ void SparsePoseGraph::AddScan(
             options_.global_sampling_ratio());
   }
 
+  // We have to check this here, because it might have changed by the time we
+  // execute the lambda.
+  const bool newly_finished_submap = insertion_submaps.front()->finished();
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(trajectory_id, matching_submap, insertion_submaps,
-                              finished_submap, pose);
+    ComputeConstraintsForScan(trajectory_id, insertion_submaps,
+                              newly_finished_submap, pose);
   });
 }
 
@@ -168,7 +170,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
   if (node_id.trajectory_id != submap_id.trajectory_id &&
       global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
     constraint_builder_.MaybeAddGlobalConstraint(
-        submap_id, submap_data_.at(submap_id).submap, node_id,
+        submap_id, submap_data_.at(submap_id).submap.get(), node_id,
         &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
         &trajectory_connectivity_);
   } else {
@@ -190,7 +192,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
               .point_cloud_pose;
 
       constraint_builder_.MaybeAddConstraint(
-          submap_id, submap_data_.at(submap_id).submap, node_id,
+          submap_id, submap_data_.at(submap_id).submap.get(), node_id,
           &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
           initial_relative_pose);
     }
@@ -216,9 +218,9 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
 }
 
 void SparsePoseGraph::ComputeConstraintsForScan(
-    const int trajectory_id, const Submap* matching_submap,
-    std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
-    const transform::Rigid2d& pose) {
+    const int trajectory_id,
+    std::vector<std::shared_ptr<const Submap>> insertion_submaps,
+    const bool newly_finished_submap, const transform::Rigid2d& pose) {
   const std::vector<mapping::SubmapId> submap_ids =
       GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
@@ -228,7 +230,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
           .at(matching_id.trajectory_id)
           .at(matching_id.submap_index)
           .pose *
-      sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
+      sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
+          .inverse() *
+      pose;
   const mapping::NodeId node_id{
       matching_id.trajectory_id,
       static_cast<size_t>(matching_id.trajectory_id) <
@@ -241,12 +245,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   optimization_problem_.AddTrajectoryNode(
       matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
-    const Submap* submap = insertion_submaps[i];
     const mapping::SubmapId submap_id = submap_ids[i];
     CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
     submap_data_.at(submap_id).node_ids.emplace(node_id);
     const transform::Rigid2d constraint_transform =
-        sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
+        sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
+        pose;
     constraints_.push_back(Constraint{submap_id,
                                       node_id,
                                       {transform::Embed3D(constraint_transform),
@@ -268,8 +272,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     }
   }
 
-  if (finished_submap != nullptr) {
-    CHECK(finished_submap == insertion_submaps.front());
+  if (newly_finished_submap) {
     const mapping::SubmapId finished_submap_id = submap_ids.front();
     SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
     CHECK(finished_submap_data.state == SubmapState::kActive);
diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h
index e21560b..ad74f71 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.h
+++ b/cartographer/mapping_2d/sparse_pose_graph.h
@@ -62,19 +62,18 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   SparsePoseGraph(const SparsePoseGraph&) = delete;
   SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
 
-  // Adds a new 'range_data_in_pose' observation at 'time', and a 'pose'
-  // that will later be optimized. The 'tracking_to_pose' is remembered so
-  // that the optimized pose can be embedded into 3D. The 'pose' was determined
-  // by scan matching against the 'matching_submap' and the scan was inserted
-  // into the 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a
-  // freshly finished submap. It is also contained in 'insertion_submaps' for
-  // the last time.
-  void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
-               const sensor::RangeData& range_data_in_pose,
-               const transform::Rigid2d& pose, int trajectory_id,
-               const Submap* matching_submap,
-               const std::vector<const Submap*>& insertion_submaps,
-               const Submap* finished_submap) EXCLUDES(mutex_);
+  // Adds a new 'range_data_in_pose' observation at 'time', and a 'pose' that
+  // will later be optimized. The 'tracking_to_pose' is remembered so that the
+  // optimized pose can be embedded into 3D. The 'pose' was determined by scan
+  // matching against the 'insertion_submaps.front()' and the scan was inserted
+  // into the 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
+  // 'true', this submap was inserted into for the last time.
+  void AddScan(
+      common::Time time, const transform::Rigid3d& tracking_to_pose,
+      const sensor::RangeData& range_data_in_pose,
+      const transform::Rigid2d& pose, int trajectory_id,
+      const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
+      EXCLUDES(mutex_);
 
   // Adds new IMU data to be used in the optimization.
   void AddImuData(int trajectory_id, common::Time time,
@@ -99,7 +98,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Likewise, all new scans are matched against submaps which are finished.
   enum class SubmapState { kActive, kFinished, kTrimmed };
   struct SubmapData {
-    const Submap* submap = nullptr;
+    std::shared_ptr<const Submap> submap;
 
     // IDs of the scans that were inserted into this map together with
     // constraints for them. They are not to be matched again when this submap
@@ -115,15 +114,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Grows the optimization problem to have an entry for every element of
   // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
   std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
-      int trajectory_id, const std::vector<const Submap*>& insertion_submaps)
+      int trajectory_id,
+      const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       REQUIRES(mutex_);
 
   // Adds constraints for a scan, and starts scan matching in the background.
-  void ComputeConstraintsForScan(int trajectory_id,
-                                 const Submap* matching_submap,
-                                 std::vector<const Submap*> insertion_submaps,
-                                 const Submap* finished_submap,
-                                 const transform::Rigid2d& pose)
+  void ComputeConstraintsForScan(
+      int trajectory_id,
+      std::vector<std::shared_ptr<const Submap>> insertion_submaps,
+      bool newly_finished_submap, const transform::Rigid2d& pose)
       REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc
index 8563176..701b4c9 100644
--- a/cartographer/mapping_2d/sparse_pose_graph_test.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc
@@ -149,9 +149,7 @@ class SparsePoseGraphTest : public ::testing::Test {
     const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
         point_cloud_,
         transform::Embed3D(current_pose_.inverse().cast<float>()));
-    const Submap* const matching_submap =
-        submaps_->Get(submaps_->matching_index());
-    std::vector<const Submap*> insertion_submaps;
+    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
     for (int insertion_index : submaps_->insertion_indices()) {
       insertion_submaps.push_back(submaps_->Get(insertion_index));
     }
@@ -162,13 +160,9 @@ class SparsePoseGraphTest : public ::testing::Test {
     submaps_->InsertRangeData(TransformRangeData(
         range_data, transform::Embed3D(pose_estimate.cast<float>())));
 
-    const Submap* const finished_submap = insertion_submaps.front()->finished()
-                                              ? insertion_submaps.front()
-                                              : nullptr;
-    sparse_pose_graph_->AddScan(common::FromUniversal(0),
-                                transform::Rigid3d::Identity(), range_data,
-                                pose_estimate, kTrajectoryId, matching_submap,
-                                insertion_submaps, finished_submap);
+    sparse_pose_graph_->AddScan(
+        common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
+        pose_estimate, kTrajectoryId, std::move(insertion_submaps));
   }
 
   void MoveRelative(const transform::Rigid2d& movement) {
diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc
index f06eccf..7800b9d 100644
--- a/cartographer/mapping_2d/submaps.cc
+++ b/cartographer/mapping_2d/submaps.cc
@@ -166,16 +166,15 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
     range_data_inserter_.Insert(range_data, &submap->probability_grid_);
     ++submap->num_range_data_;
   }
-  const Submap* const last_submap = Get(size() - 1);
-  if (last_submap->num_range_data_ == options_.num_range_data()) {
+  if (submaps_.back()->num_range_data_ == options_.num_range_data()) {
     AddSubmap(range_data.origin.head<2>());
   }
 }
 
-const Submap* Submaps::Get(int index) const {
+std::shared_ptr<const Submap> Submaps::Get(int index) const {
   CHECK_GE(index, 0);
   CHECK_LT(index, size());
-  return submaps_[index].get();
+  return submaps_[index];
 }
 
 int Submaps::size() const { return submaps_.size(); }
diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h
index 96c748e..ba5bd62 100644
--- a/cartographer/mapping_2d/submaps.h
+++ b/cartographer/mapping_2d/submaps.h
@@ -79,7 +79,7 @@ class Submaps {
   Submaps(const Submaps&) = delete;
   Submaps& operator=(const Submaps&) = delete;
 
-  const Submap* Get(int index) const;
+  std::shared_ptr<const Submap> Get(int index) const;
   int size() const;
 
   // Returns the index of the newest initialized Submap which can be
@@ -98,8 +98,7 @@ class Submaps {
   void AddSubmap(const Eigen::Vector2f& origin);
 
   const proto::SubmapsOptions options_;
-
-  std::vector<std::unique_ptr<Submap>> submaps_;
+  std::vector<std::shared_ptr<Submap>> submaps_;
   RangeDataInserter range_data_inserter_;
 };
 
diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc
index d548945..aabcee6 100644
--- a/cartographer/mapping_3d/global_trajectory_builder.cc
+++ b/cartographer/mapping_3d/global_trajectory_builder.cc
@@ -58,16 +58,10 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
   if (insertion_result == nullptr) {
     return;
   }
-
-  const Submap* const finished_submap =
-      insertion_result->insertion_submaps.front()->finished()
-          ? insertion_result->insertion_submaps.front()
-          : nullptr;
   sparse_pose_graph_->AddScan(
       insertion_result->time, insertion_result->range_data_in_tracking,
       insertion_result->pose_observation, trajectory_id_,
-      insertion_result->matching_submap, insertion_result->insertion_submaps,
-      finished_submap);
+      std::move(insertion_result->insertion_submaps));
 }
 
 void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc
index 01c72e5..4775964 100644
--- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc
+++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc
@@ -140,7 +140,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
   pose_tracker_->GetPoseEstimateMeanAndCovariance(
       time, &pose_prediction, &unused_covariance_prediction);
 
-  const Submap* const matching_submap =
+  std::shared_ptr<const Submap> matching_submap =
       submaps_->Get(submaps_->matching_index());
   transform::Rigid3d initial_ceres_pose =
       matching_submap->local_pose().inverse() * pose_prediction;
@@ -219,9 +219,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
   if (motion_filter_.IsSimilar(time, pose_observation)) {
     return nullptr;
   }
-  const Submap* const matching_submap =
-      submaps_->Get(submaps_->matching_index());
-  std::vector<const Submap*> insertion_submaps;
+  std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
@@ -231,7 +229,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
       pose_tracker_->gravity_orientation());
   return std::unique_ptr<InsertionResult>(
       new InsertionResult{time, range_data_in_tracking, pose_observation,
-                          matching_submap, insertion_submaps});
+                          std::move(insertion_submaps)});
 }
 
 }  // namespace mapping_3d
diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h
index 4731d53..2e116d1 100644
--- a/cartographer/mapping_3d/local_trajectory_builder_interface.h
+++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h
@@ -37,8 +37,7 @@ class LocalTrajectoryBuilderInterface {
     common::Time time;
     sensor::RangeData range_data_in_tracking;
     transform::Rigid3d pose_observation;
-    const Submap* matching_submap;
-    std::vector<const Submap*> insertion_submaps;
+    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
 
   virtual ~LocalTrajectoryBuilderInterface() {}
diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
index 4bdfbcf..b1bfd29 100644
--- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
+++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
@@ -236,7 +236,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
   }
 
   ceres::Problem problem;
-  const Submap* const matching_submap =
+  std::shared_ptr<const Submap> matching_submap =
       submaps_->Get(submaps_->matching_index());
   // We transform the states in 'batches_' in place to be in the submap frame as
   // expected by the OccupiedSpaceCostFunctor. This is reverted after solving
@@ -409,9 +409,7 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
   if (motion_filter_.IsSimilar(time, pose_observation)) {
     return nullptr;
   }
-  const Submap* const matching_submap =
-      submaps_->Get(submaps_->matching_index());
-  std::vector<const Submap*> insertion_submaps;
+  std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
@@ -425,7 +423,7 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
 
   return std::unique_ptr<InsertionResult>(
       new InsertionResult{time, range_data_in_tracking, pose_observation,
-                          matching_submap, insertion_submaps});
+                          std::move(insertion_submaps)});
 }
 
 OptimizingLocalTrajectoryBuilder::State
diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 028fe33..2595e89 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -54,7 +54,7 @@ SparsePoseGraph::~SparsePoseGraph() {
 
 std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
     const int trajectory_id,
-    const std::vector<const Submap*>& insertion_submaps) {
+    const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   CHECK(!insertion_submaps.empty());
   const auto& submap_data = optimization_problem_.submap_data();
   if (insertion_submaps.size() == 1) {
@@ -95,12 +95,9 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 void SparsePoseGraph::AddScan(
     common::Time time, const sensor::RangeData& range_data_in_tracking,
     const transform::Rigid3d& pose, const int trajectory_id,
-    const Submap* const matching_submap,
-    const std::vector<const Submap*>& insertion_submaps,
-    const Submap* const finished_submap) {
+    const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * pose);
-
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
       trajectory_id,
@@ -133,9 +130,12 @@ void SparsePoseGraph::AddScan(
             options_.global_sampling_ratio());
   }
 
+  // We have to check this here, because it might have changed by the time we
+  // execute the lambda.
+  const bool newly_finished_submap = insertion_submaps.front()->finished();
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(trajectory_id, matching_submap, insertion_submaps,
-                              finished_submap, pose);
+    ComputeConstraintsForScan(trajectory_id, insertion_submaps,
+                              newly_finished_submap, pose);
   });
 }
 
@@ -197,7 +197,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
     // yaw component will be handled by the matching procedure in the
     // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
     constraint_builder_.MaybeAddGlobalConstraint(
-        submap_id, submap_data_.at(submap_id).submap, node_id,
+        submap_id, submap_data_.at(submap_id).submap.get(), node_id,
         &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
         submap_nodes, initial_relative_pose.rotation(),
         &trajectory_connectivity_);
@@ -210,7 +210,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
     if (node_id.trajectory_id == submap_id.trajectory_id ||
         scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
-          submap_id, submap_data_.at(submap_id).submap, node_id,
+          submap_id, submap_data_.at(submap_id).submap.get(), node_id,
           &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
           submap_nodes, initial_relative_pose);
     }
@@ -235,18 +235,19 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
 }
 
 void SparsePoseGraph::ComputeConstraintsForScan(
-    const int trajectory_id, const Submap* matching_submap,
-    std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
-    const transform::Rigid3d& pose) {
+    const int trajectory_id,
+    std::vector<std::shared_ptr<const Submap>> insertion_submaps,
+    const bool newly_finished_submap, const transform::Rigid3d& pose) {
   const std::vector<mapping::SubmapId> submap_ids =
       GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
+  CHECK_EQ(submap_ids.size(), insertion_submaps.size());
   const mapping::SubmapId matching_id = submap_ids.front();
   const transform::Rigid3d optimized_pose =
       optimization_problem_.submap_data()
           .at(matching_id.trajectory_id)
           .at(matching_id.submap_index)
           .pose *
-      matching_submap->local_pose().inverse() * pose;
+      insertion_submaps.front()->local_pose().inverse() * pose;
   const mapping::NodeId node_id{
       matching_id.trajectory_id,
       static_cast<size_t>(matching_id.trajectory_id) <
@@ -259,12 +260,11 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
                                           scan_data->time, optimized_pose);
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
-    const Submap* submap = insertion_submaps[i];
     const mapping::SubmapId submap_id = submap_ids[i];
     CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
     submap_data_.at(submap_id).node_ids.emplace(node_id);
     const transform::Rigid3d constraint_transform =
-        submap->local_pose().inverse() * pose;
+        insertion_submaps[i]->local_pose().inverse() * pose;
     constraints_.push_back(
         Constraint{submap_id,
                    node_id,
@@ -286,8 +286,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     }
   }
 
-  if (finished_submap != nullptr) {
-    CHECK(finished_submap == insertion_submaps.front());
+  if (newly_finished_submap) {
     const mapping::SubmapId finished_submap_id = submap_ids.front();
     SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
     CHECK(finished_submap_data.state == SubmapState::kActive);
diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h
index c9c876b..8c1b217 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.h
+++ b/cartographer/mapping_3d/sparse_pose_graph.h
@@ -21,6 +21,7 @@
 #include <functional>
 #include <limits>
 #include <map>
+#include <memory>
 #include <unordered_map>
 #include <vector>
 
@@ -62,16 +63,14 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
   // that will later be optimized. The 'pose' was determined by scan matching
-  // against the 'matching_submap' and the scan was inserted into the
-  // 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a freshly
-  // finished submap. It is also contained in 'insertion_submaps' for the last
-  // time.
-  void AddScan(common::Time time,
-               const sensor::RangeData& range_data_in_tracking,
-               const transform::Rigid3d& pose, int trajectory_id,
-               const Submap* matching_submap,
-               const std::vector<const Submap*>& insertion_submaps,
-               const Submap* finished_submap) EXCLUDES(mutex_);
+  // against 'insertion_submaps.front()' and the scan was inserted into the
+  // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is 'true',
+  // this submap was inserted into for the last time.
+  void AddScan(
+      common::Time time, const sensor::RangeData& range_data_in_tracking,
+      const transform::Rigid3d& pose, int trajectory_id,
+      const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
+      EXCLUDES(mutex_);
 
   // Adds new IMU data to be used in the optimization.
   void AddImuData(int trajectory_id, common::Time time,
@@ -98,7 +97,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Likewise, all new scans are matched against submaps which are finished.
   enum class SubmapState { kActive, kFinished, kTrimmed };
   struct SubmapData {
-    const Submap* submap = nullptr;
+    std::shared_ptr<const Submap> submap;
 
     // IDs of the scans that were inserted into this map together with
     // constraints for them. They are not to be matched again when this submap
@@ -114,15 +113,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Grows the optimization problem to have an entry for every element of
   // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
   std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
-      int trajectory_id, const std::vector<const Submap*>& insertion_submaps)
+      int trajectory_id,
+      const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       REQUIRES(mutex_);
 
   // Adds constraints for a scan, and starts scan matching in the background.
-  void ComputeConstraintsForScan(int trajectory_id,
-                                 const Submap* matching_submap,
-                                 std::vector<const Submap*> insertion_submaps,
-                                 const Submap* finished_submap,
-                                 const transform::Rigid3d& pose)
+  void ComputeConstraintsForScan(
+      int trajectory_id,
+      std::vector<std::shared_ptr<const Submap>> insertion_submaps,
+      bool newly_finished_submap, const transform::Rigid3d& pose)
       REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc
index c5cde7c..b17a799 100644
--- a/cartographer/mapping_3d/submaps.cc
+++ b/cartographer/mapping_3d/submaps.cc
@@ -371,10 +371,10 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
   AddSubmap(transform::Rigid3d::Identity());
 }
 
-const Submap* Submaps::Get(int index) const {
+std::shared_ptr<const Submap> Submaps::Get(int index) const {
   CHECK_GE(index, 0);
   CHECK_LT(index, size());
-  return submaps_[index].get();
+  return submaps_[index];
 }
 
 int Submaps::size() const { return submaps_.size(); }
@@ -407,8 +407,7 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data,
                                 &submap->low_resolution_hybrid_grid_);
     ++submap->num_range_data_;
   }
-  const Submap* const last_submap = Get(size() - 1);
-  if (last_submap->num_range_data_ == options_.num_range_data()) {
+  if (submaps_.back()->num_range_data_ == options_.num_range_data()) {
     AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
                                  gravity_alignment));
   }
diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h
index c8f51b4..5096720 100644
--- a/cartographer/mapping_3d/submaps.h
+++ b/cartographer/mapping_3d/submaps.h
@@ -92,7 +92,7 @@ class Submaps {
   Submaps(const Submaps&) = delete;
   Submaps& operator=(const Submaps&) = delete;
 
-  const Submap* Get(int index) const;
+  std::shared_ptr<const Submap> Get(int index) const;
   int size() const;
 
   // Returns the index of the newest initialized Submap which can be
@@ -113,10 +113,7 @@ class Submaps {
   void AddSubmap(const transform::Rigid3d& local_pose);
 
   const proto::SubmapsOptions options_;
-
-  // 'submaps_' contains pointers, so that resizing the vector does not
-  // invalidate handed out Submap* pointers.
-  std::vector<std::unique_ptr<Submap>> submaps_;
+  std::vector<std::shared_ptr<Submap>> submaps_;
   RangeDataInserter range_data_inserter_;
 };