diff --git a/cartographer/cloud/internal/map_builder_context.cc b/cartographer/cloud/internal/map_builder_context.cc
deleted file mode 100644
index 6631b17..0000000
--- a/cartographer/cloud/internal/map_builder_context.cc
+++ /dev/null
@@ -1,188 +0,0 @@
-/*
- * Copyright 2018 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "cartographer/cloud/internal/map_builder_context.h"
-
-#include "cartographer/cloud/internal/map_builder_server.h"
-#include "cartographer/mapping/internal/2d/local_slam_result_2d.h"
-#include "cartographer/mapping/internal/3d/local_slam_result_3d.h"
-
-namespace cartographer {
-namespace cloud {
-
-MapBuilderContext::MapBuilderContext(MapBuilderServer* map_builder_server)
-    : map_builder_server_(map_builder_server) {}
-
-mapping::MapBuilderInterface& MapBuilderContext::map_builder() {
-  return *map_builder_server_->map_builder_;
-}
-
-common::BlockingQueue<std::unique_ptr<MapBuilderContextInterface::Data>>&
-MapBuilderContext::sensor_data_queue() {
-  return map_builder_server_->incoming_data_queue_;
-}
-
-mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
-MapBuilderContext::GetLocalSlamResultCallbackForSubscriptions() {
-  MapBuilderServer* map_builder_server = map_builder_server_;
-  return [map_builder_server](
-             int trajectory_id, common::Time time,
-             transform::Rigid3d local_pose, sensor::RangeData range_data,
-             std::unique_ptr<
-                 const mapping::TrajectoryBuilderInterface::InsertionResult>
-                 insertion_result) {
-    map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose,
-                                          std::move(range_data),
-                                          std::move(insertion_result));
-  };
-}
-
-void MapBuilderContext::AddSensorDataToTrajectory(const Data& sensor_data) {
-  sensor_data.data->AddToTrajectoryBuilder(
-      map_builder_server_->map_builder_->GetTrajectoryBuilder(
-          sensor_data.trajectory_id));
-}
-
-MapBuilderContextInterface::SubscriptionId
-MapBuilderContext::SubscribeLocalSlamResults(
-    int trajectory_id, LocalSlamSubscriptionCallback callback) {
-  return map_builder_server_->SubscribeLocalSlamResults(trajectory_id,
-                                                        callback);
-}
-
-void MapBuilderContext::UnsubscribeLocalSlamResults(
-    const SubscriptionId& subscription_id) {
-  map_builder_server_->UnsubscribeLocalSlamResults(subscription_id);
-}
-
-void MapBuilderContext::NotifyFinishTrajectory(int trajectory_id) {
-  map_builder_server_->NotifyFinishTrajectory(trajectory_id);
-}
-
-std::shared_ptr<mapping::Submap2D> MapBuilderContext::UpdateSubmap2D(
-    const mapping::proto::Submap& proto) {
-  CHECK(proto.has_submap_2d());
-  mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
-                              proto.submap_id().submap_index()};
-  std::shared_ptr<mapping::Submap2D> submap_2d_ptr;
-  auto submap_it = unfinished_submaps_.find(submap_id);
-  if (submap_it == unfinished_submaps_.end()) {
-    // Seeing a submap for the first time it should never be finished.
-    CHECK(!proto.submap_2d().finished());
-    submap_2d_ptr = std::make_shared<mapping::Submap2D>(proto.submap_2d());
-    unfinished_submaps_.Insert(submap_id, submap_2d_ptr);
-  } else {
-    submap_2d_ptr =
-        std::dynamic_pointer_cast<mapping::Submap2D>(submap_it->data);
-    CHECK(submap_2d_ptr);
-    submap_2d_ptr->UpdateFromProto(proto);
-
-    // If the submap was just finished by the recent update, remove it from the
-    // list of unfinished submaps.
-    if (submap_2d_ptr->finished()) {
-      unfinished_submaps_.Trim(submap_id);
-    } else {
-      // If the submap is unfinished set the 'num_range_data' to 0 since we
-      // haven't changed the HybridGrid.
-      submap_2d_ptr->set_num_range_data(0);
-    }
-  }
-  return submap_2d_ptr;
-}
-
-std::shared_ptr<mapping::Submap3D> MapBuilderContext::UpdateSubmap3D(
-    const mapping::proto::Submap& proto) {
-  CHECK(proto.has_submap_3d());
-  mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
-                              proto.submap_id().submap_index()};
-  std::shared_ptr<mapping::Submap3D> submap_3d_ptr;
-  auto submap_it = unfinished_submaps_.find(submap_id);
-  if (submap_it == unfinished_submaps_.end()) {
-    // Seeing a submap for the first time it should never be finished.
-    CHECK(!proto.submap_3d().finished());
-    submap_3d_ptr = std::make_shared<mapping::Submap3D>(proto.submap_3d());
-    unfinished_submaps_.Insert(submap_id, submap_3d_ptr);
-    submap_it = unfinished_submaps_.find(submap_id);
-  } else {
-    submap_3d_ptr =
-        std::dynamic_pointer_cast<mapping::Submap3D>(submap_it->data);
-    CHECK(submap_3d_ptr);
-
-    // Update submap with information in incoming request.
-    submap_3d_ptr->UpdateFromProto(proto);
-
-    // If the submap was just finished by the recent update, remove it from the
-    // list of unfinished submaps.
-    if (submap_3d_ptr->finished()) {
-      unfinished_submaps_.Trim(submap_id);
-    } else {
-      // If the submap is unfinished set the 'num_range_data' to 0 since we
-      // haven't changed the HybridGrid.
-      submap_3d_ptr->set_num_range_data(0);
-    }
-  }
-  return submap_3d_ptr;
-}
-
-std::unique_ptr<mapping::LocalSlamResultData>
-MapBuilderContext::ProcessLocalSlamResultData(
-    const std::string& sensor_id, common::Time time,
-    const mapping::proto::LocalSlamResultData& proto) {
-  CHECK_GE(proto.submaps().size(), 1);
-  CHECK(proto.submaps(0).has_submap_2d() || proto.submaps(0).has_submap_3d());
-  if (proto.submaps(0).has_submap_2d()) {
-    std::vector<std::shared_ptr<const mapping::Submap2D>> submaps;
-    for (const auto& submap_proto : proto.submaps()) {
-      submaps.push_back(UpdateSubmap2D(submap_proto));
-    }
-    return common::make_unique<mapping::LocalSlamResult2D>(
-        sensor_id, time,
-        std::make_shared<const mapping::TrajectoryNode::Data>(
-            mapping::FromProto(proto.node_data())),
-        submaps);
-  } else {
-    std::vector<std::shared_ptr<const mapping::Submap3D>> submaps;
-    for (const auto& submap_proto : proto.submaps()) {
-      submaps.push_back(UpdateSubmap3D(submap_proto));
-    }
-    return common::make_unique<mapping::LocalSlamResult3D>(
-        sensor_id, time,
-        std::make_shared<const mapping::TrajectoryNode::Data>(
-            mapping::FromProto(proto.node_data())),
-        std::move(submaps));
-  }
-}
-
-LocalTrajectoryUploaderInterface*
-MapBuilderContext::local_trajectory_uploader() {
-  return map_builder_server_->local_trajectory_uploader_.get();
-}
-
-void MapBuilderContext::EnqueueSensorData(int trajectory_id,
-                                          std::unique_ptr<sensor::Data> data) {
-  map_builder_server_->incoming_data_queue_.Push(
-      common::make_unique<Data>(Data{trajectory_id, std::move(data)}));
-}
-
-void MapBuilderContext::EnqueueLocalSlamResultData(
-    int trajectory_id, const std::string& sensor_id,
-    std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) {
-  map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
-      Data{trajectory_id, std::move(local_slam_result_data)}));
-}
-
-}  // namespace cloud
-}  // namespace cartographer
diff --git a/cartographer/cloud/internal/map_builder_context.h b/cartographer/cloud/internal/map_builder_context.h
deleted file mode 100644
index f0a65b4..0000000
--- a/cartographer/cloud/internal/map_builder_context.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * Copyright 2018 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef CARTOGRAPHER_INTERNAL_CLOUD_MAP_BUILDER_CONTEXT_H
-#define CARTOGRAPHER_INTERNAL_CLOUD_MAP_BUILDER_CONTEXT_H
-
-#include "cartographer/cloud/internal/map_builder_context_interface.h"
-#include "cartographer/mapping/2d/submap_2d.h"
-#include "cartographer/mapping/3d/submap_3d.h"
-
-namespace cartographer {
-namespace cloud {
-
-class MapBuilderContext : public MapBuilderContextInterface {
- public:
-  MapBuilderContext(MapBuilderServer* map_builder_server);
-  mapping::MapBuilderInterface& map_builder() override;
-  common::BlockingQueue<std::unique_ptr<Data>>& sensor_data_queue() override;
-  mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
-  GetLocalSlamResultCallbackForSubscriptions() override;
-  void AddSensorDataToTrajectory(const Data& sensor_data) override;
-  SubscriptionId SubscribeLocalSlamResults(
-      int trajectory_id, LocalSlamSubscriptionCallback callback) override;
-  void UnsubscribeLocalSlamResults(
-      const SubscriptionId& subscription_id) override;
-  void NotifyFinishTrajectory(int trajectory_id) override;
-  std::unique_ptr<mapping::LocalSlamResultData> ProcessLocalSlamResultData(
-      const std::string& sensor_id, common::Time time,
-      const mapping::proto::LocalSlamResultData& proto) override;
-  LocalTrajectoryUploaderInterface* local_trajectory_uploader() override;
-  void EnqueueSensorData(int trajectory_id,
-                         std::unique_ptr<sensor::Data> data) override;
-  void EnqueueLocalSlamResultData(int trajectory_id,
-                                  const std::string& sensor_id,
-                                  std::unique_ptr<mapping::LocalSlamResultData>
-                                      local_slam_result_data) override;
-
- private:
-  std::shared_ptr<mapping::Submap2D> UpdateSubmap2D(
-      const mapping::proto::Submap& proto);
-  std::shared_ptr<mapping::Submap3D> UpdateSubmap3D(
-      const mapping::proto::Submap& proto);
-
-  MapBuilderServer* map_builder_server_;
-  mapping::MapById<mapping::SubmapId, std::shared_ptr<mapping::Submap>>
-      unfinished_submaps_;
-};
-
-}  // namespace cloud
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_INTERNAL_CLOUD_MAP_BUILDER_CONTEXT_H
diff --git a/cartographer/cloud/internal/map_builder_context_impl.cc b/cartographer/cloud/internal/map_builder_context_impl.cc
new file mode 100644
index 0000000..ef6eaab
--- /dev/null
+++ b/cartographer/cloud/internal/map_builder_context_impl.cc
@@ -0,0 +1,63 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/cloud/internal/map_builder_server.h"
+
+#include "cartographer/cloud/internal/map_builder_server.h"
+#include "cartographer/mapping/internal/2d/local_slam_result_2d.h"
+#include "cartographer/mapping/internal/3d/local_slam_result_3d.h"
+
+namespace cartographer {
+namespace cloud {
+
+template <>
+std::unique_ptr<mapping::LocalSlamResultData>
+MapBuilderContext<mapping::Submap2D>::ProcessLocalSlamResultData(
+    const std::string& sensor_id, common::Time time,
+    const mapping::proto::LocalSlamResultData& proto) {
+  CHECK_GE(proto.submaps().size(), 1);
+  CHECK(proto.submaps(0).has_submap_2d());
+  std::vector<std::shared_ptr<const mapping::Submap2D>> submaps;
+  for (const auto& submap_proto : proto.submaps()) {
+    submaps.push_back(submap_controller_.UpdateSubmap(submap_proto));
+  }
+  return common::make_unique<mapping::LocalSlamResult2D>(
+      sensor_id, time,
+      std::make_shared<const mapping::TrajectoryNode::Data>(
+          mapping::FromProto(proto.node_data())),
+      submaps);
+}
+
+template <>
+std::unique_ptr<mapping::LocalSlamResultData>
+MapBuilderContext<mapping::Submap3D>::ProcessLocalSlamResultData(
+    const std::string& sensor_id, common::Time time,
+    const mapping::proto::LocalSlamResultData& proto) {
+  CHECK_GE(proto.submaps().size(), 1);
+  CHECK(proto.submaps(0).has_submap_3d());
+  std::vector<std::shared_ptr<const mapping::Submap3D>> submaps;
+  for (const auto& submap_proto : proto.submaps()) {
+    submaps.push_back(submap_controller_.UpdateSubmap(submap_proto));
+  }
+  return common::make_unique<mapping::LocalSlamResult3D>(
+      sensor_id, time,
+      std::make_shared<const mapping::TrajectoryNode::Data>(
+          mapping::FromProto(proto.node_data())),
+      submaps);
+}
+
+}  // namespace cloud
+}  // namespace cartographer
diff --git a/cartographer/cloud/internal/map_builder_context_impl.h b/cartographer/cloud/internal/map_builder_context_impl.h
new file mode 100644
index 0000000..0913a0f
--- /dev/null
+++ b/cartographer/cloud/internal/map_builder_context_impl.h
@@ -0,0 +1,117 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_INTERNAL_CLOUD_MAP_BUILDER_CONTEXT_IMPL_H
+#define CARTOGRAPHER_INTERNAL_CLOUD_MAP_BUILDER_CONTEXT_IMPL_H
+
+namespace cartographer {
+namespace cloud {
+
+template <class SubmapType>
+MapBuilderContext<SubmapType>::MapBuilderContext(
+    MapBuilderServer* map_builder_server)
+    : map_builder_server_(map_builder_server) {}
+
+template <class SubmapType>
+mapping::MapBuilderInterface& MapBuilderContext<SubmapType>::map_builder() {
+  return *map_builder_server_->map_builder_;
+}
+
+template <class SubmapType>
+common::BlockingQueue<std::unique_ptr<MapBuilderContextInterface::Data>>&
+MapBuilderContext<SubmapType>::sensor_data_queue() {
+  return map_builder_server_->incoming_data_queue_;
+}
+
+template <class SubmapType>
+mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
+MapBuilderContext<SubmapType>::GetLocalSlamResultCallbackForSubscriptions() {
+  MapBuilderServer* map_builder_server = map_builder_server_;
+  return [map_builder_server](
+             int trajectory_id, common::Time time,
+             transform::Rigid3d local_pose, sensor::RangeData range_data,
+             std::unique_ptr<
+                 const mapping::TrajectoryBuilderInterface::InsertionResult>
+                 insertion_result) {
+    map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose,
+                                          std::move(range_data),
+                                          std::move(insertion_result));
+  };
+}
+
+template <class SubmapType>
+void MapBuilderContext<SubmapType>::AddSensorDataToTrajectory(
+    const Data& sensor_data) {
+  sensor_data.data->AddToTrajectoryBuilder(
+      map_builder_server_->map_builder_->GetTrajectoryBuilder(
+          sensor_data.trajectory_id));
+}
+
+template <class SubmapType>
+MapBuilderContextInterface::SubscriptionId
+MapBuilderContext<SubmapType>::SubscribeLocalSlamResults(
+    int trajectory_id, LocalSlamSubscriptionCallback callback) {
+  return map_builder_server_->SubscribeLocalSlamResults(trajectory_id,
+                                                        callback);
+}
+
+template <class SubmapType>
+void MapBuilderContext<SubmapType>::UnsubscribeLocalSlamResults(
+    const SubscriptionId& subscription_id) {
+  map_builder_server_->UnsubscribeLocalSlamResults(subscription_id);
+}
+
+template <class SubmapType>
+void MapBuilderContext<SubmapType>::NotifyFinishTrajectory(int trajectory_id) {
+  map_builder_server_->NotifyFinishTrajectory(trajectory_id);
+}
+
+template <class SubmapType>
+LocalTrajectoryUploaderInterface*
+MapBuilderContext<SubmapType>::local_trajectory_uploader() {
+  return map_builder_server_->local_trajectory_uploader_.get();
+}
+
+template <class SubmapType>
+void MapBuilderContext<SubmapType>::EnqueueSensorData(
+    int trajectory_id, std::unique_ptr<sensor::Data> data) {
+  map_builder_server_->incoming_data_queue_.Push(
+      common::make_unique<Data>(Data{trajectory_id, std::move(data)}));
+}
+
+template <class SubmapType>
+void MapBuilderContext<SubmapType>::EnqueueLocalSlamResultData(
+    int trajectory_id, const std::string& sensor_id,
+    std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) {
+  map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
+      Data{trajectory_id, std::move(local_slam_result_data)}));
+}
+
+template <>
+std::unique_ptr<mapping::LocalSlamResultData>
+MapBuilderContext<mapping::Submap2D>::ProcessLocalSlamResultData(
+    const std::string& sensor_id, common::Time time,
+    const mapping::proto::LocalSlamResultData& proto);
+template <>
+std::unique_ptr<mapping::LocalSlamResultData>
+MapBuilderContext<mapping::Submap3D>::ProcessLocalSlamResultData(
+    const std::string& sensor_id, common::Time time,
+    const mapping::proto::LocalSlamResultData& proto);
+
+}  // namespace cloud
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_INTERNAL_CLOUD_MAP_BUILDER_CONTEXT_IMPL_H
diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc
index 7fcf4be..5a4ee06 100644
--- a/cartographer/cloud/internal/map_builder_server.cc
+++ b/cartographer/cloud/internal/map_builder_server.cc
@@ -82,8 +82,18 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::RunFinalOptimizationHandler>();
   server_builder.RegisterHandler<handlers::WriteStateHandler>();
   grpc_server_ = server_builder.Build();
-  grpc_server_->SetExecutionContext(
-      common::make_unique<MapBuilderContext>(this));
+  if (map_builder_server_options.map_builder_options()
+          .use_trajectory_builder_2d()) {
+    grpc_server_->SetExecutionContext(
+        common::make_unique<MapBuilderContext<mapping::Submap2D>>(this));
+  } else if (map_builder_server_options.map_builder_options()
+                 .use_trajectory_builder_3d()) {
+    grpc_server_->SetExecutionContext(
+        common::make_unique<MapBuilderContext<mapping::Submap3D>>(this));
+  } else {
+    LOG(FATAL)
+        << "Set either use_trajectory_builder_2d or use_trajectory_builder_3d";
+  }
 }
 
 void MapBuilderServer::Start() {
@@ -119,8 +129,8 @@ void MapBuilderServer::ProcessSensorDataQueue() {
     std::unique_ptr<MapBuilderContextInterface::Data> sensor_data =
         incoming_data_queue_.PopWithTimeout(kPopTimeout);
     if (sensor_data) {
-      grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
-          *sensor_data);
+      grpc_server_->GetContext<MapBuilderContextInterface>()
+          ->AddSensorDataToTrajectory(*sensor_data);
     }
   }
 }
@@ -144,13 +154,14 @@ void MapBuilderServer::OnLocalSlamResult(
   // If there is an uplink server and a submap insertion happened, enqueue this
   // local SLAM result for uploading.
   if (insertion_result &&
-      grpc_server_->GetUnsynchronizedContext<MapBuilderContext>()
+      grpc_server_->GetUnsynchronizedContext<MapBuilderContextInterface>()
           ->local_trajectory_uploader()) {
     auto data_request =
         common::make_unique<proto::AddLocalSlamResultDataRequest>();
-    auto sensor_id = grpc_server_->GetUnsynchronizedContext<MapBuilderContext>()
-                         ->local_trajectory_uploader()
-                         ->GetLocalSlamResultSensorId(trajectory_id);
+    auto sensor_id =
+        grpc_server_->GetUnsynchronizedContext<MapBuilderContextInterface>()
+            ->local_trajectory_uploader()
+            ->GetLocalSlamResultSensorId(trajectory_id);
     CreateAddLocalSlamResultDataRequest(sensor_id.id, trajectory_id, time,
                                         starting_submap_index_,
                                         *insertion_result, data_request.get());
@@ -158,7 +169,7 @@ void MapBuilderServer::OnLocalSlamResult(
     if (insertion_result->insertion_submaps.front()->finished()) {
       ++starting_submap_index_;
     }
-    grpc_server_->GetUnsynchronizedContext<MapBuilderContext>()
+    grpc_server_->GetUnsynchronizedContext<MapBuilderContextInterface>()
         ->local_trajectory_uploader()
         ->EnqueueDataRequest(std::move(data_request));
   }
diff --git a/cartographer/cloud/internal/map_builder_server.h b/cartographer/cloud/internal/map_builder_server.h
index ea6c75f..029f3f1 100644
--- a/cartographer/cloud/internal/map_builder_server.h
+++ b/cartographer/cloud/internal/map_builder_server.h
@@ -20,11 +20,14 @@
 #include "async_grpc/execution_context.h"
 #include "async_grpc/server.h"
 #include "cartographer/cloud/internal/local_trajectory_uploader.h"
-#include "cartographer/cloud/internal/map_builder_context.h"
+#include "cartographer/cloud/internal/map_builder_context_interface.h"
+#include "cartographer/cloud/internal/submap_controller.h"
 #include "cartographer/cloud/map_builder_server_interface.h"
 #include "cartographer/cloud/proto/map_builder_server_options.pb.h"
 #include "cartographer/common/blocking_queue.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/2d/submap_2d.h"
+#include "cartographer/mapping/3d/submap_3d.h"
 #include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/mapping/map_builder.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
@@ -33,9 +36,44 @@
 namespace cartographer {
 namespace cloud {
 
+class MapBuilderServer;
+
+template <class SubmapType>
+class MapBuilderContext : public MapBuilderContextInterface {
+ public:
+  MapBuilderContext(MapBuilderServer* map_builder_server);
+  mapping::MapBuilderInterface& map_builder() override;
+  common::BlockingQueue<std::unique_ptr<MapBuilderContextInterface::Data>>&
+  sensor_data_queue() override;
+  mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
+  GetLocalSlamResultCallbackForSubscriptions() override;
+  void AddSensorDataToTrajectory(const Data& sensor_data) override;
+  MapBuilderContextInterface::SubscriptionId SubscribeLocalSlamResults(
+      int trajectory_id, LocalSlamSubscriptionCallback callback) override;
+  void UnsubscribeLocalSlamResults(
+      const SubscriptionId& subscription_id) override;
+  void NotifyFinishTrajectory(int trajectory_id) override;
+  std::unique_ptr<mapping::LocalSlamResultData> ProcessLocalSlamResultData(
+      const std::string& sensor_id, common::Time time,
+      const mapping::proto::LocalSlamResultData& proto) override;
+  LocalTrajectoryUploaderInterface* local_trajectory_uploader() override;
+
+  void EnqueueSensorData(int trajectory_id,
+                         std::unique_ptr<sensor::Data> data) override;
+  void EnqueueLocalSlamResultData(int trajectory_id,
+                                  const std::string& sensor_id,
+                                  std::unique_ptr<mapping::LocalSlamResultData>
+                                      local_slam_result_data) override;
+
+ private:
+  MapBuilderServer* map_builder_server_;
+  SubmapController<SubmapType> submap_controller_;
+};
+
 class MapBuilderServer : public MapBuilderServerInterface {
  public:
-  friend MapBuilderContext;
+  friend MapBuilderContext<mapping::Submap2D>;
+  friend MapBuilderContext<mapping::Submap3D>;
 
   MapBuilderServer(
       const proto::MapBuilderServerOptions& map_builder_server_options,
@@ -94,4 +132,6 @@ class MapBuilderServer : public MapBuilderServerInterface {
 }  // namespace cloud
 }  // namespace cartographer
 
+#include "cartographer/cloud/internal/map_builder_context_impl.h"
+
 #endif  // CARTOGRAPHER_CLOUD_INTERNAL_MAP_BUILDER_SERVER_H
diff --git a/cartographer/cloud/internal/submap_controller.cc b/cartographer/cloud/internal/submap_controller.cc
new file mode 100644
index 0000000..ea263c0
--- /dev/null
+++ b/cartographer/cloud/internal/submap_controller.cc
@@ -0,0 +1,37 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/cloud/internal/submap_controller.h"
+
+namespace cartographer {
+namespace cloud {
+
+template <>
+std::shared_ptr<mapping::Submap2D>
+SubmapController<mapping::Submap2D>::CreateSubmap(
+    const mapping::proto::Submap& proto) {
+  return std::make_shared<mapping::Submap2D>(proto.submap_2d());
+}
+
+template <>
+std::shared_ptr<mapping::Submap3D>
+SubmapController<mapping::Submap3D>::CreateSubmap(
+    const mapping::proto::Submap& proto) {
+  return std::make_shared<mapping::Submap3D>(proto.submap_3d());
+}
+
+}  // namespace cloud
+}  // namespace cartographer
\ No newline at end of file
diff --git a/cartographer/cloud/internal/submap_controller.h b/cartographer/cloud/internal/submap_controller.h
new file mode 100644
index 0000000..056e335
--- /dev/null
+++ b/cartographer/cloud/internal/submap_controller.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H
+#define CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H
+
+#include "cartographer/mapping/2d/submap_2d.h"
+#include "cartographer/mapping/3d/submap_3d.h"
+#include "cartographer/mapping/id.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
+
+namespace cartographer {
+namespace cloud {
+
+template <class SubmapType>
+class SubmapController {
+ public:
+  std::shared_ptr<SubmapType> UpdateSubmap(
+      const mapping::proto::Submap& proto) {
+    mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
+                                proto.submap_id().submap_index()};
+    std::shared_ptr<SubmapType> submap_ptr;
+    auto submap_it = unfinished_submaps_.find(submap_id);
+    if (submap_it == unfinished_submaps_.end()) {
+      submap_ptr = CreateSubmap(proto);
+      unfinished_submaps_.Insert(submap_id, submap_ptr);
+      return submap_ptr;
+    }
+    submap_ptr = submap_it->data;
+    CHECK(submap_ptr);
+    submap_ptr->UpdateFromProto(proto);
+
+    // If the submap was just finished by the recent update, remove it from
+    // the list of unfinished submaps.
+    if (submap_ptr->finished()) {
+      unfinished_submaps_.Trim(submap_id);
+    } else {
+      // If the submap is unfinished set the 'num_range_data' to 0 since we
+      // haven't changed the HybridGrid.
+      submap_ptr->set_num_range_data(0);
+    }
+    return submap_ptr;
+  }
+
+ private:
+  std::shared_ptr<SubmapType> CreateSubmap(const mapping::proto::Submap& proto);
+
+  mapping::MapById<mapping::SubmapId, std::shared_ptr<SubmapType>>
+      unfinished_submaps_;
+};
+
+template <>
+std::shared_ptr<mapping::Submap2D>
+SubmapController<mapping::Submap2D>::CreateSubmap(
+    const mapping::proto::Submap& proto);
+template <>
+std::shared_ptr<mapping::Submap3D>
+SubmapController<mapping::Submap3D>::CreateSubmap(
+    const mapping::proto::Submap& proto);
+
+}  // namespace cloud
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H