diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h
index 47b8cd4..9797f25 100644
--- a/cartographer/mapping/submaps.h
+++ b/cartographer/mapping/submaps.h
@@ -65,7 +65,7 @@ class Submap {
   transform::Rigid3d local_pose() const { return local_pose_; }
 
   // Number of RangeData inserted.
-  size_t num_range_data() const { return num_range_data_; }
+  int num_range_data() const { return num_range_data_; }
 
   // Fills data into the 'response'.
   virtual void ToResponseProto(
diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc
index 8becb1b..74d6ba8 100644
--- a/cartographer/mapping_2d/submaps.cc
+++ b/cartographer/mapping_2d/submaps.cc
@@ -151,6 +151,19 @@ void Submap::ToResponseProto(
       transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
 }
 
+void Submap::InsertRangeData(const sensor::RangeData& range_data,
+                             const RangeDataInserter& range_data_inserter) {
+  CHECK(!finished_);
+  range_data_inserter.Insert(range_data, &probability_grid_);
+  ++num_range_data_;
+}
+
+void Submap::Finish() {
+  CHECK(!finished_);
+  probability_grid_ = ComputeCroppedProbabilityGrid(probability_grid_);
+  finished_ = true;
+}
+
 ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
     : options_(options),
       range_data_inserter_(options.range_data_inserter_options()) {
@@ -161,11 +174,9 @@ ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
 
 void ActiveSubmaps::InsertRangeData(const sensor::RangeData& range_data) {
   for (auto& submap : submaps_) {
-    CHECK(!submap->finished_);
-    range_data_inserter_.Insert(range_data, &submap->probability_grid_);
-    ++submap->num_range_data_;
+    submap->InsertRangeData(range_data, range_data_inserter_);
   }
-  if (submaps_.back()->num_range_data_ == options_.num_range_data()) {
+  if (submaps_.back()->num_range_data() == options_.num_range_data()) {
     AddSubmap(range_data.origin.head<2>());
   }
 }
@@ -177,17 +188,12 @@ std::vector<std::shared_ptr<Submap>> ActiveSubmaps::submaps() const {
 int ActiveSubmaps::matching_index() const { return matching_submap_index_; }
 
 void ActiveSubmaps::FinishSubmap() {
-  // Crop the finished Submap before inserting a new Submap to reduce peak
-  // memory usage a bit.
   Submap* submap = submaps_.front().get();
-  CHECK(!submap->finished_);
-  submap->probability_grid_ =
-      ComputeCroppedProbabilityGrid(submap->probability_grid_);
-  submap->finished_ = true;
+  submap->Finish();
   if (options_.output_debug_images()) {
     // Output the Submap that won't be changed from now on.
     WriteDebugImage("submap" + std::to_string(matching_submap_index_) + ".webp",
-                    submaps_.front()->probability_grid_);
+                    submaps_.front()->probability_grid());
   }
   ++matching_submap_index_;
   submaps_.erase(submaps_.begin());
@@ -195,6 +201,8 @@ void ActiveSubmaps::FinishSubmap() {
 
 void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) {
   if (submaps_.size() > 1) {
+    // This will crop the finished Submap before inserting a new Submap to
+    // reduce peak memory usage a bit.
     FinishSubmap();
   }
   const int num_cells_per_dimension =
diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h
index f8f8e4b..b4e093d 100644
--- a/cartographer/mapping_2d/submaps.h
+++ b/cartographer/mapping_2d/submaps.h
@@ -52,10 +52,13 @@ class Submap : public mapping::Submap {
       const transform::Rigid3d& global_submap_pose,
       mapping::proto::SubmapQuery::Response* response) const override;
 
- private:
-  // TODO(hrapp): Remove friend declaration.
-  friend class ActiveSubmaps;
+  // Insert 'range_data' into this submap using 'range_data_inserter'. The
+  // submap must not be finished yet.
+  void InsertRangeData(const sensor::RangeData& range_data,
+                       const RangeDataInserter& range_data_inserter);
+  void Finish();
 
+ private:
   ProbabilityGrid probability_grid_;
   bool finished_ = false;
 };
diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc
index b3731d0..2192d64 100644
--- a/cartographer/mapping_3d/submaps.cc
+++ b/cartographer/mapping_3d/submaps.cc
@@ -360,6 +360,26 @@ void Submap::ToResponseProto(
           global_submap_pose.translation().z())));
 }
 
+void Submap::InsertRangeData(const sensor::RangeData& range_data,
+                             const RangeDataInserter& range_data_inserter,
+                             const int high_resolution_max_range) {
+  CHECK(!finished_);
+  const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
+      range_data, local_pose().inverse().cast<float>());
+  range_data_inserter.Insert(
+      FilterRangeDataByMaxRange(transformed_range_data,
+                                high_resolution_max_range),
+      &high_resolution_hybrid_grid_);
+  range_data_inserter.Insert(transformed_range_data,
+                             &low_resolution_hybrid_grid_);
+  ++num_range_data_;
+}
+
+void Submap::Finish() {
+  CHECK(!finished_);
+  finished_ = true;
+}
+
 ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
     : options_(options),
       range_data_inserter_(options.range_data_inserter_options()) {
@@ -381,17 +401,10 @@ void ActiveSubmaps::InsertRangeData(
     const sensor::RangeData& range_data,
     const Eigen::Quaterniond& gravity_alignment) {
   for (auto& submap : submaps_) {
-    const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
-        range_data, submap->local_pose().inverse().cast<float>());
-    range_data_inserter_.Insert(
-        FilterRangeDataByMaxRange(transformed_range_data,
-                                  options_.high_resolution_max_range()),
-        &submap->high_resolution_hybrid_grid_);
-    range_data_inserter_.Insert(transformed_range_data,
-                                &submap->low_resolution_hybrid_grid_);
-    ++submap->num_range_data_;
+    submap->InsertRangeData(range_data, range_data_inserter_,
+                            options_.high_resolution_max_range());
   }
-  if (submaps_.back()->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));
   }
@@ -399,9 +412,7 @@ void ActiveSubmaps::InsertRangeData(
 
 void ActiveSubmaps::AddSubmap(const transform::Rigid3d& local_pose) {
   if (submaps_.size() > 1) {
-    Submap* submap = submaps_.front().get();
-    CHECK(!submap->finished_);
-    submap->finished_ = true;
+    submaps_.front()->Finish();
     ++matching_submap_index_;
     submaps_.erase(submaps_.begin());
   }
diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h
index 13f3f00..b7b2097 100644
--- a/cartographer/mapping_3d/submaps.h
+++ b/cartographer/mapping_3d/submaps.h
@@ -64,10 +64,14 @@ class Submap : public mapping::Submap {
       const transform::Rigid3d& global_submap_pose,
       mapping::proto::SubmapQuery::Response* response) const override;
 
- private:
-  // TODO(hrapp): Remove friend declaration.
-  friend class ActiveSubmaps;
+  // Insert 'range_data' into this submap using 'range_data_inserter'. The
+  // submap must not be finished yet.
+  void InsertRangeData(const sensor::RangeData& range_data,
+                       const RangeDataInserter& range_data_inserter,
+                       int high_resolution_max_range);
+  void Finish();
 
+ private:
   HybridGrid high_resolution_hybrid_grid_;
   HybridGrid low_resolution_hybrid_grid_;
   bool finished_ = false;