Introduce PoseGraphInterface. (#744)

[RFC=0005](https://github.com/googlecartographer/rfcs/blob/master/text/0005-pose-graph-interface.md)

cartographer/mapping/map_builder.cc

@@ -307,7 +307,7 @@ int MapBuilder::num_trajectory_builders() const {
   return trajectory_builders_.size();
 }
 
-PoseGraph* MapBuilder::pose_graph() { return pose_graph_; }
+PoseGraphInterface* MapBuilder::pose_graph() { return pose_graph_; }
 
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/map_builder.h

@@ -23,6 +23,7 @@
 #include <unordered_map>
 
 #include "cartographer/common/thread_pool.h"
+#include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/mapping/proto/map_builder_options.pb.h"
 #include "cartographer/mapping_2d/pose_graph.h"
 #include "cartographer/mapping_3d/pose_graph.h"
@@ -65,7 +66,7 @@ class MapBuilder : public MapBuilderInterface {
 
   int num_trajectory_builders() const override;
 
-  mapping::PoseGraph* pose_graph() override;
+  mapping::PoseGraphInterface* pose_graph() override;
 
  private:
   const proto::MapBuilderOptions options_;

cartographer/mapping/map_builder_interface.h

@@ -27,7 +27,7 @@
 #include "cartographer/io/proto_stream.h"
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
 #include "cartographer/mapping/id.h"
-#include "cartographer/mapping/pose_graph.h"
+#include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
 #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
 #include "cartographer/mapping/submaps.h"
@@ -43,8 +43,8 @@ class MapBuilderInterface {
   using LocalSlamResultCallback =
       GlobalTrajectoryBuilderInterface::LocalSlamResultCallback;
 
-  MapBuilderInterface(){};
-  virtual ~MapBuilderInterface(){};
+  MapBuilderInterface() {}
+  virtual ~MapBuilderInterface() {}
 
   MapBuilderInterface(const MapBuilderInterface&) = delete;
   MapBuilderInterface& operator=(const MapBuilderInterface&) = delete;
@@ -82,7 +82,7 @@ class MapBuilderInterface {
 
   virtual int num_trajectory_builders() const = 0;
 
-  virtual mapping::PoseGraph* pose_graph() = 0;
+  virtual mapping::PoseGraphInterface* pose_graph() = 0;
 };
 
 }  // namespace mapping

cartographer/mapping/pose_graph.h

@@ -25,6 +25,7 @@
 
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/mapping/id.h"
+#include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/mapping/pose_graph_trimmer.h"
 #include "cartographer/mapping/proto/pose_graph.pb.h"
 #include "cartographer/mapping/proto/pose_graph_options.pb.h"
@@ -43,35 +44,8 @@ namespace mapping {
 proto::PoseGraphOptions CreatePoseGraphOptions(
     common::LuaParameterDictionary* const parameter_dictionary);
 
-class PoseGraph {
+class PoseGraph : public PoseGraphInterface {
  public:
-  // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
-  // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
-  // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
-  struct Constraint {
-    struct Pose {
-      transform::Rigid3d zbar_ij;
-      double translation_weight;
-      double rotation_weight;
-    };
-
-    SubmapId submap_id;  // 'i' in the paper.
-    NodeId node_id;      // 'j' in the paper.
-
-    // Pose of the node 'j' relative to submap 'i'.
-    Pose pose;
-
-    // Differentiates between intra-submap (where node 'j' was inserted into
-    // submap 'i') and inter-submap constraints (where node 'j' was not inserted
-    // into submap 'i').
-    enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
-  };
-
-  struct SubmapData {
-    std::shared_ptr<const Submap> submap;
-    transform::Rigid3d pose;
-  };
-
   struct InitialTrajectoryPose {
     int to_trajectory_id;
     transform::Rigid3d relative_pose;
@@ -79,7 +53,7 @@ class PoseGraph {
   };
 
   PoseGraph() {}
-  virtual ~PoseGraph() {}
+  virtual ~PoseGraph() override {}
 
   PoseGraph(const PoseGraph&) = delete;
   PoseGraph& operator=(const PoseGraph&) = delete;
@@ -100,9 +74,6 @@ class PoseGraph {
   // Finishes the given trajectory.
   virtual void FinishTrajectory(int trajectory_id) = 0;
 
-  // Checks if the given trajectory is finished.
-  virtual bool IsTrajectoryFinished(int trajectory_id) = 0;
-
   // Freezes a trajectory. Poses in this trajectory will not be optimized.
   virtual void FreezeTrajectory(int trajectory_id) = 0;
 
@@ -130,9 +101,6 @@ class PoseGraph {
   // included in the pose graph.
   virtual void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) = 0;
 
-  // Computes optimized poses.
-  virtual void RunFinalOptimization() = 0;
-
   // Gets the current trajectory clusters.
   virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
 
@@ -141,17 +109,6 @@ class PoseGraph {
   // not exist (anymore).
   virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
 
-  // Returns data for all submaps.
-  virtual MapById<SubmapId, SubmapData> GetAllSubmapData() = 0;
-
-  // Returns the transform converting data in the local map frame (i.e. the
-  // continuous, non-loop-closed frame) into the global map frame (i.e. the
-  // discontinuous, loop-closed frame).
-  virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0;
-
-  // Returns the current optimized trajectories.
-  virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0;
-
   // Serializes the constraints and trajectories.
   proto::PoseGraph ToProto();
 
@@ -165,9 +122,6 @@ class PoseGraph {
   virtual sensor::MapByTime<sensor::FixedFramePoseData>
   GetFixedFramePoseData() = 0;
 
-  // Returns the collection of constraints.
-  virtual std::vector<Constraint> constraints() = 0;
-
   // Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
   // respect to 'to_trajectory_id' at time 'time'.
   virtual void SetInitialTrajectoryPose(int from_trajectory_id,

cartographer/mapping/pose_graph_interface.h

@@ -0,0 +1,87 @@
+/*
+ * Copyright 2016 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_MAPPING_POSE_GRAPH_INTERFACE_H_
+#define CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_
+
+#include <vector>
+
+#include "cartographer/mapping/id.h"
+#include "cartographer/mapping/submaps.h"
+
+namespace cartographer {
+namespace mapping {
+
+class PoseGraphInterface {
+ public:
+  // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
+  // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
+  // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
+  struct Constraint {
+    struct Pose {
+      transform::Rigid3d zbar_ij;
+      double translation_weight;
+      double rotation_weight;
+    };
+
+    SubmapId submap_id;  // 'i' in the paper.
+    NodeId node_id;      // 'j' in the paper.
+
+    // Pose of the node 'j' relative to submap 'i'.
+    Pose pose;
+
+    // Differentiates between intra-submap (where node 'j' was inserted into
+    // submap 'i') and inter-submap constraints (where node 'j' was not inserted
+    // into submap 'i').
+    enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
+  };
+
+  struct SubmapData {
+    std::shared_ptr<const Submap> submap;
+    transform::Rigid3d pose;
+  };
+
+  PoseGraphInterface() {}
+  virtual ~PoseGraphInterface() {}
+
+  PoseGraphInterface(const PoseGraphInterface&) = delete;
+  PoseGraphInterface& operator=(const PoseGraphInterface&) = delete;
+
+  // Computes optimized poses.
+  virtual void RunFinalOptimization() = 0;
+
+  // Returns data for all submaps.
+  virtual MapById<SubmapId, SubmapData> GetAllSubmapData() = 0;
+
+  // Returns the transform converting data in the local map frame (i.e. the
+  // continuous, non-loop-closed frame) into the global map frame (i.e. the
+  // discontinuous, loop-closed frame).
+  virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0;
+
+  // Returns the current optimized trajectories.
+  virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0;
+
+  // Checks if the given trajectory is finished.
+  virtual bool IsTrajectoryFinished(int trajectory_id) = 0;
+
+  // Returns the collection of constraints.
+  virtual std::vector<Constraint> constraints() = 0;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_POSE_GRAPH_INTERFACE_H_