From b29986f29785a1fa224df9c877338dd300a393cf Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Thu, 8 Mar 2018 14:49:41 +0100
Subject: [PATCH] Remove the pointers to the derived PoseGraph*D classes from
 map_builder. (#966)

---
 cartographer/mapping/map_builder.cc | 26 +++++++++++++++-----------
 cartographer/mapping/map_builder.h  |  7 ++-----
 2 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc
index d5c90fa..3ceff97 100644
--- a/cartographer/mapping/map_builder.cc
+++ b/cartographer/mapping/map_builder.cc
@@ -24,6 +24,8 @@
 
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/2d/pose_graph_2d.h"
+#include "cartographer/mapping/3d/pose_graph_3d.h"
 #include "cartographer/mapping/collated_trajectory_builder.h"
 #include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h"
 #include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h"
@@ -58,14 +60,12 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
 MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
     : options_(options), thread_pool_(options.num_background_threads()) {
   if (options.use_trajectory_builder_2d()) {
-    pose_graph_2d_ = common::make_unique<PoseGraph2D>(
+    pose_graph_ = common::make_unique<PoseGraph2D>(
         options_.pose_graph_options(), &thread_pool_);
-    pose_graph_ = pose_graph_2d_.get();
   }
   if (options.use_trajectory_builder_3d()) {
-    pose_graph_3d_ = common::make_unique<PoseGraph3D>(
+    pose_graph_ = common::make_unique<PoseGraph3D>(
         options_.pose_graph_options(), &thread_pool_);
-    pose_graph_ = pose_graph_3d_.get();
   }
   if (options.collate_by_trajectory()) {
     sensor_collator_ = common::make_unique<sensor::TrajectoryCollator>();
@@ -85,24 +85,28 @@ int MapBuilder::AddTrajectoryBuilder(
       local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder3D>(
           trajectory_options.trajectory_builder_3d_options());
     }
+    DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));
     trajectory_builders_.push_back(
         common::make_unique<CollatedTrajectoryBuilder>(
             sensor_collator_.get(), trajectory_id, expected_sensor_ids,
-            CreateGlobalTrajectoryBuilder3D(std::move(local_trajectory_builder),
-                                            trajectory_id, pose_graph_3d_.get(),
-                                            local_slam_result_callback)));
+            CreateGlobalTrajectoryBuilder3D(
+                std::move(local_trajectory_builder), trajectory_id,
+                static_cast<PoseGraph3D*>(pose_graph_.get()),
+                local_slam_result_callback)));
   } else {
     std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
     if (trajectory_options.has_trajectory_builder_2d_options()) {
       local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(
           trajectory_options.trajectory_builder_2d_options());
     }
+    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
     trajectory_builders_.push_back(
         common::make_unique<CollatedTrajectoryBuilder>(
             sensor_collator_.get(), trajectory_id, expected_sensor_ids,
-            CreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder),
-                                            trajectory_id, pose_graph_2d_.get(),
-                                            local_slam_result_callback)));
+            CreateGlobalTrajectoryBuilder2D(
+                std::move(local_trajectory_builder), trajectory_id,
+                static_cast<PoseGraph2D*>(pose_graph_.get()),
+                local_slam_result_callback)));
   }
   if (trajectory_options.pure_localization()) {
     constexpr int kSubmapsToKeep = 3;
@@ -411,7 +415,7 @@ int MapBuilder::num_trajectory_builders() const {
   return trajectory_builders_.size();
 }
 
-PoseGraphInterface* MapBuilder::pose_graph() { return pose_graph_; }
+PoseGraphInterface* MapBuilder::pose_graph() { return pose_graph_.get(); }
 
 const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>&
 MapBuilder::GetAllTrajectoryBuilderOptions() const {
diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h
index 8833f3b..9659b97 100644
--- a/cartographer/mapping/map_builder.h
+++ b/cartographer/mapping/map_builder.h
@@ -23,8 +23,7 @@
 #include <set>
 
 #include "cartographer/common/thread_pool.h"
-#include "cartographer/mapping/2d/pose_graph_2d.h"
-#include "cartographer/mapping/3d/pose_graph_3d.h"
+#include "cartographer/mapping/pose_graph.h"
 #include "cartographer/mapping/proto/map_builder_options.pb.h"
 #include "cartographer/sensor/collator_interface.h"
 
@@ -77,9 +76,7 @@ class MapBuilder : public MapBuilderInterface {
   const proto::MapBuilderOptions options_;
   common::ThreadPool thread_pool_;
 
-  std::unique_ptr<PoseGraph2D> pose_graph_2d_;
-  std::unique_ptr<PoseGraph3D> pose_graph_3d_;
-  mapping::PoseGraph* pose_graph_;
+  std::unique_ptr<PoseGraph> pose_graph_;
 
   std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
   std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>