OptimizationProblemInterface (#991)

Derive optimization problems from a new OptimizationProblemInterface.
Move fix_z into OptimizationProblemOptions.
Move trivial getters to header files.
This will allow mocking the optimization problems.

cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h

@@ -40,9 +40,8 @@ class LandmarkCostFunction2D {
       PoseGraphInterface::LandmarkNode::LandmarkObservation;
 
   static ceres::CostFunction* CreateAutoDiffCostFunction(
-      const LandmarkObservation& observation,
+      const LandmarkObservation& observation, const NodeData2D& prev_node,
-      const OptimizationProblem2D::NodeData& prev_node,
+      const NodeData2D& next_node) {
-      const OptimizationProblem2D::NodeData& next_node) {
     return new ceres::AutoDiffCostFunction<
         LandmarkCostFunction2D, 6 /* residuals */,
         3 /* previous node pose variables */, 3 /* next node pose variables */,
@@ -72,8 +71,8 @@ class LandmarkCostFunction2D {
 
  private:
   LandmarkCostFunction2D(const LandmarkObservation& observation,
-                         const OptimizationProblem2D::NodeData& prev_node,
+                         const NodeData2D& prev_node,
-                         const OptimizationProblem2D::NodeData& next_node)
+                         const NodeData2D& next_node)
       : landmark_to_tracking_transform_(
             observation.landmark_to_tracking_transform),
         prev_node_gravity_alignment_(prev_node.gravity_alignment),

cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc

@@ -34,10 +34,10 @@ using LandmarkObservation =
     PoseGraphInterface::LandmarkNode::LandmarkObservation;
 
 TEST(LandmarkCostFunctionTest, SmokeTest) {
-  OptimizationProblem2D::NodeData prev_node;
+  NodeData2D prev_node;
   prev_node.time = common::FromUniversal(0);
   prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
-  OptimizationProblem2D::NodeData next_node;
+  NodeData2D next_node;
   next_node.time = common::FromUniversal(10);
   next_node.gravity_alignment = Eigen::Quaterniond::Identity();
 

cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc

@@ -42,8 +42,8 @@ namespace {
 
 using ::cartographer::mapping::pose_graph::CeresPose;
 using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
-using NodeData = OptimizationProblem2D::NodeData;
+using NodeData = NodeData2D;
-using SubmapData = OptimizationProblem2D::SubmapData;
+using SubmapData = SubmapData2D;
 
 // Converts a pose into the 3 optimization variable format used for Ceres:
 // translation in x and y, followed by the rotation angle representing the
@@ -149,20 +149,14 @@ void OptimizationProblem2D::AddOdometryData(
   odometry_data_.Append(trajectory_id, odometry_data);
 }
 
-void OptimizationProblem2D::AddTrajectoryNode(
+void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
-    const int trajectory_id, const common::Time time,
+                                              const NodeData& node_data) {
-    const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
+  node_data_.Append(trajectory_id, node_data);
-    const Eigen::Quaterniond& gravity_alignment) {
-  node_data_.Append(trajectory_id,
-                    NodeData{time, initial_pose, pose, gravity_alignment});
 }
 
-void OptimizationProblem2D::InsertTrajectoryNode(
+void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id,
-    const NodeId& node_id, const common::Time time,
+                                                 const NodeData& node_data) {
-    const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
+  node_data_.Insert(node_id, node_data);
-    const Eigen::Quaterniond& gravity_alignment) {
-  node_data_.Insert(node_id,
-                    NodeData{time, initial_pose, pose, gravity_alignment});
 }
 
 void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
@@ -301,30 +295,6 @@ void OptimizationProblem2D::Solve(
   }
 }
 
-const MapById<NodeId, NodeData>& OptimizationProblem2D::node_data() const {
-  return node_data_;
-}
-
-const MapById<SubmapId, SubmapData>& OptimizationProblem2D::submap_data()
-    const {
-  return submap_data_;
-}
-
-const std::map<std::string, transform::Rigid3d>&
-OptimizationProblem2D::landmark_data() const {
-  return landmark_data_;
-}
-
-const sensor::MapByTime<sensor::ImuData>& OptimizationProblem2D::imu_data()
-    const {
-  return imu_data_;
-}
-
-const sensor::MapByTime<sensor::OdometryData>&
-OptimizationProblem2D::odometry_data() const {
-  return odometry_data_;
-}
-
 std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
     const int trajectory_id, const common::Time time) const {
   const auto it = odometry_data_.lower_bound(trajectory_id, time);

cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h

@@ -28,6 +28,7 @@
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/id.h"
+#include "cartographer/mapping/internal/pose_graph/optimization_problem_interface.h"
 #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
 #include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/sensor/imu_data.h"
@@ -39,23 +40,21 @@ namespace cartographer {
 namespace mapping {
 namespace pose_graph {
 
-// Implements the SPA loop closure method.
+struct NodeData2D {
-class OptimizationProblem2D {
- public:
-  using Constraint = PoseGraphInterface::Constraint;
-  using LandmarkNode = PoseGraphInterface::LandmarkNode;
-
-  struct NodeData {
   common::Time time;
   transform::Rigid2d initial_pose;
   transform::Rigid2d pose;
   Eigen::Quaterniond gravity_alignment;
 };
 
-  struct SubmapData {
+struct SubmapData2D {
   transform::Rigid2d global_pose;
 };
 
+class OptimizationProblem2D
+    : public OptimizationProblemInterface<NodeData2D, SubmapData2D,
+                                          transform::Rigid2d> {
+ public:
   explicit OptimizationProblem2D(
       const pose_graph::proto::OptimizationProblemOptions& options);
   ~OptimizationProblem2D();
@@ -63,48 +62,55 @@ class OptimizationProblem2D {
   OptimizationProblem2D(const OptimizationProblem2D&) = delete;
   OptimizationProblem2D& operator=(const OptimizationProblem2D&) = delete;
 
-  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override;
   void AddOdometryData(int trajectory_id,
-                       const sensor::OdometryData& odometry_data);
+                       const sensor::OdometryData& odometry_data) override;
-  void AddTrajectoryNode(int trajectory_id, common::Time time,
+  void AddTrajectoryNode(int trajectory_id,
-                         const transform::Rigid2d& initial_pose,
+                         const NodeData2D& node_data) override;
-                         const transform::Rigid2d& pose,
+  void InsertTrajectoryNode(const NodeId& node_id,
-                         const Eigen::Quaterniond& gravity_alignment);
+                            const NodeData2D& node_data) override;
-  void InsertTrajectoryNode(const NodeId& node_id, common::Time time,
+  void TrimTrajectoryNode(const NodeId& node_id) override;
-                            const transform::Rigid2d& initial_pose,
-                            const transform::Rigid2d& pose,
-                            const Eigen::Quaterniond& gravity_alignment);
-  void TrimTrajectoryNode(const NodeId& node_id);
   void AddSubmap(int trajectory_id,
-                 const transform::Rigid2d& global_submap_pose);
+                 const transform::Rigid2d& global_submap_pose) override;
   void InsertSubmap(const SubmapId& submap_id,
-                    const transform::Rigid2d& global_submap_pose);
+                    const transform::Rigid2d& global_submap_pose) override;
-  void TrimSubmap(const SubmapId& submap_id);
+  void TrimSubmap(const SubmapId& submap_id) override;
+  void SetMaxNumIterations(int32 max_num_iterations) override;
 
-  void SetMaxNumIterations(int32 max_num_iterations);
+  void Solve(
-
+      const std::vector<Constraint>& constraints,
-  // Optimizes the global poses.
-  void Solve(const std::vector<Constraint>& constraints,
       const std::set<int>& frozen_trajectories,
-             const std::map<std::string, LandmarkNode>& landmark_nodes);
+      const std::map<std::string, LandmarkNode>& landmark_nodes) override;
 
-  const MapById<NodeId, NodeData>& node_data() const;
+  const MapById<NodeId, NodeData2D>& node_data() const override {
-  const MapById<SubmapId, SubmapData>& submap_data() const;
+    return node_data_;
-  const std::map<std::string, transform::Rigid3d>& landmark_data() const;
+  }
-  const sensor::MapByTime<sensor::ImuData>& imu_data() const;
+  const MapById<SubmapId, SubmapData2D>& submap_data() const override {
-  const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
+    return submap_data_;
+  }
+  const std::map<std::string, transform::Rigid3d>& landmark_data()
+      const override {
+    return landmark_data_;
+  }
+  const sensor::MapByTime<sensor::ImuData>& imu_data() const override {
+    return imu_data_;
+  }
+  const sensor::MapByTime<sensor::OdometryData>& odometry_data()
+      const override {
+    return odometry_data_;
+  }
 
  private:
   std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
       int trajectory_id, common::Time time) const;
   // Uses odometry if available, otherwise the local SLAM results.
   transform::Rigid3d ComputeRelativePose(
-      int trajectory_id, const NodeData& first_node_data,
+      int trajectory_id, const NodeData2D& first_node_data,
-      const NodeData& second_node_data) const;
+      const NodeData2D& second_node_data) const;
 
   pose_graph::proto::OptimizationProblemOptions options_;
-  MapById<NodeId, NodeData> node_data_;
+  MapById<NodeId, NodeData2D> node_data_;
-  MapById<SubmapId, SubmapData> submap_data_;
+  MapById<SubmapId, SubmapData2D> submap_data_;
   std::map<std::string, transform::Rigid3d> landmark_data_;
   sensor::MapByTime<sensor::ImuData> imu_data_;
   sensor::MapByTime<sensor::OdometryData> odometry_data_;

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -246,8 +246,9 @@ void PoseGraph2D::ComputeConstraintsForNode(
       pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
       pose;
   optimization_problem_.AddTrajectoryNode(
-      matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
+      matching_id.trajectory_id,
-      constant_data->gravity_alignment);
+      pose_graph::NodeData2D{constant_data->time, pose, optimized_pose,
+                             constant_data->gravity_alignment});
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const SubmapId submap_id = submap_ids[i];
     // Even if this was the last node added to 'submap_id', the submap will
@@ -440,9 +441,8 @@ void PoseGraph2D::AddSubmapFromProto(
   submap_data_.Insert(submap_id, SubmapData());
   submap_data_.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the 'global_submap_pose'.
-  global_submap_poses_.Insert(
+  global_submap_poses_.Insert(submap_id,
-      submap_id,
+                              pose_graph::SubmapData2D{global_submap_pose_2d});
-      pose_graph::OptimizationProblem2D::SubmapData{global_submap_pose_2d});
   AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
     submap_data_.at(submap_id).state = SubmapState::kFinished;
     optimization_problem_.InsertSubmap(submap_id, global_submap_pose_2d);
@@ -465,11 +465,13 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
     const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
         constant_data->gravity_alignment.inverse());
     optimization_problem_.InsertTrajectoryNode(
-        node_id, constant_data->time,
+        node_id,
+        pose_graph::NodeData2D{
+            constant_data->time,
             transform::Project2D(constant_data->local_pose *
                                  gravity_alignment_inverse),
             transform::Project2D(global_pose * gravity_alignment_inverse),
-        constant_data->gravity_alignment);
+            constant_data->gravity_alignment});
   });
 }
 
@@ -728,8 +730,7 @@ PoseGraph2D::GetAllSubmapPoses() {
 }
 
 transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
-    const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
+    const MapById<SubmapId, pose_graph::SubmapData2D>& global_submap_poses,
-        global_submap_poses,
     const int trajectory_id) const {
   auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
   auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);

cartographer/mapping/internal/2d/pose_graph_2d.h

@@ -191,8 +191,7 @@ class PoseGraph2D : public PoseGraph {
   // Computes the local to global map frame transform based on the given
   // 'global_submap_poses'.
   transform::Rigid3d ComputeLocalToGlobalTransform(
-      const MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>&
+      const MapById<SubmapId, pose_graph::SubmapData2D>& global_submap_poses,
-          global_submap_poses,
       int trajectory_id) const REQUIRES(mutex_);
 
   PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
@@ -244,8 +243,8 @@ class PoseGraph2D : public PoseGraph {
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Global submap poses currently used for displaying data.
-  MapById<SubmapId, pose_graph::OptimizationProblem2D::SubmapData>
+  MapById<SubmapId, pose_graph::SubmapData2D> global_submap_poses_
-      global_submap_poses_ GUARDED_BY(mutex_);
+      GUARDED_BY(mutex_);
 
   // Global landmark poses with all observations.
   std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>

cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h

@@ -39,9 +39,8 @@ class LandmarkCostFunction3D {
       PoseGraphInterface::LandmarkNode::LandmarkObservation;
 
   static ceres::CostFunction* CreateAutoDiffCostFunction(
-      const LandmarkObservation& observation,
+      const LandmarkObservation& observation, const NodeData3D& prev_node,
-      const OptimizationProblem3D::NodeData& prev_node,
+      const NodeData3D& next_node) {
-      const OptimizationProblem3D::NodeData& next_node) {
     return new ceres::AutoDiffCostFunction<
         LandmarkCostFunction3D, 6 /* residuals */,
         4 /* previous node rotation variables */,
@@ -77,8 +76,8 @@ class LandmarkCostFunction3D {
 
  private:
   LandmarkCostFunction3D(const LandmarkObservation& observation,
-                         const OptimizationProblem3D::NodeData& prev_node,
+                         const NodeData3D& prev_node,
-                         const OptimizationProblem3D::NodeData& next_node)
+                         const NodeData3D& next_node)
       : landmark_to_tracking_transform_(
             observation.landmark_to_tracking_transform),
         translation_weight_(observation.translation_weight),

cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc

@@ -34,9 +34,9 @@ using LandmarkObservation =
     PoseGraphInterface::LandmarkNode::LandmarkObservation;
 
 TEST(LandmarkCostFunction3DTest, SmokeTest) {
-  OptimizationProblem3D::NodeData prev_node;
+  NodeData3D prev_node;
   prev_node.time = common::FromUniversal(0);
-  OptimizationProblem3D::NodeData next_node;
+  NodeData3D next_node;
   next_node.time = common::FromUniversal(10);
 
   std::unique_ptr<ceres::CostFunction> cost_function(

cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc

@@ -52,8 +52,8 @@ namespace {
 using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
 using TrajectoryData =
     ::cartographer::mapping::PoseGraphInterface::TrajectoryData;
-using NodeData = OptimizationProblem3D::NodeData;
+using NodeData = NodeData3D;
-using SubmapData = OptimizationProblem3D::SubmapData;
+using SubmapData = SubmapData3D;
 
 // For odometry.
 std::unique_ptr<transform::Rigid3d> Interpolate(
@@ -179,8 +179,8 @@ void AddLandmarkCostFunctions(
 }  // namespace
 
 OptimizationProblem3D::OptimizationProblem3D(
-    const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z)
+    const pose_graph::proto::OptimizationProblemOptions& options)
-    : options_(options), fix_z_(fix_z) {}
+    : options_(options) {}
 
 OptimizationProblem3D::~OptimizationProblem3D() {}
 
@@ -200,11 +200,9 @@ void OptimizationProblem3D::AddFixedFramePoseData(
   fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
 }
 
-void OptimizationProblem3D::AddTrajectoryNode(
+void OptimizationProblem3D::AddTrajectoryNode(const int trajectory_id,
-    const int trajectory_id, const common::Time time,
+                                              const NodeData& node_data) {
-    const transform::Rigid3d& local_pose,
+  node_data_.Append(trajectory_id, node_data);
-    const transform::Rigid3d& global_pose) {
-  node_data_.Append(trajectory_id, NodeData{time, local_pose, global_pose});
   trajectory_data_[trajectory_id];
 }
 
@@ -213,11 +211,9 @@ void OptimizationProblem3D::SetTrajectoryData(
   trajectory_data_[trajectory_id] = trajectory_data;
 }
 
-void OptimizationProblem3D::InsertTrajectoryNode(
+void OptimizationProblem3D::InsertTrajectoryNode(const NodeId& node_id,
-    const NodeId& node_id, const common::Time time,
+                                                 const NodeData& node_data) {
-    const transform::Rigid3d& local_pose,
+  node_data_.Insert(node_id, node_data);
-    const transform::Rigid3d& global_pose) {
-  node_data_.Insert(node_id, NodeData{time, local_pose, global_pose});
   trajectory_data_[node_id.trajectory_id];
 }
 
@@ -265,7 +261,7 @@ void OptimizationProblem3D::Solve(
 
   const auto translation_parameterization =
       [this]() -> std::unique_ptr<ceres::LocalParameterization> {
-    return fix_z_ == FixZ::kYes
+    return options_.fix_z_in_3d()
                ? common::make_unique<ceres::SubsetParameterization>(
                      3, std::vector<int>{2})
                : nullptr;
@@ -342,7 +338,7 @@ void OptimizationProblem3D::Solve(
                            &C_nodes, &C_landmarks, &problem);
   // Add constraints based on IMU observations of angular velocities and
   // linear acceleration.
-  if (fix_z_ == FixZ::kNo) {
+  if (!options_.fix_z_in_3d()) {
     for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
       const int trajectory_id = node_it->id.trajectory_id;
       const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
@@ -429,7 +425,7 @@ void OptimizationProblem3D::Solve(
     }
   }
 
-  if (fix_z_ == FixZ::kYes) {
+  if (options_.fix_z_in_3d()) {
     // Add penalties for violating odometry or changes between consecutive nodes
     // if odometry is not available.
     for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
@@ -565,40 +561,6 @@ void OptimizationProblem3D::Solve(
   }
 }
 
-const MapById<NodeId, NodeData>& OptimizationProblem3D::node_data() const {
-  return node_data_;
-}
-
-const MapById<SubmapId, SubmapData>& OptimizationProblem3D::submap_data()
-    const {
-  return submap_data_;
-}
-
-const std::map<std::string, transform::Rigid3d>&
-OptimizationProblem3D::landmark_data() const {
-  return landmark_data_;
-}
-
-const sensor::MapByTime<sensor::ImuData>& OptimizationProblem3D::imu_data()
-    const {
-  return imu_data_;
-}
-
-const sensor::MapByTime<sensor::OdometryData>&
-OptimizationProblem3D::odometry_data() const {
-  return odometry_data_;
-}
-
-const sensor::MapByTime<sensor::FixedFramePoseData>&
-OptimizationProblem3D::fixed_frame_pose_data() const {
-  return fixed_frame_pose_data_;
-}
-
-const std::map<int, TrajectoryData>& OptimizationProblem3D::trajectory_data()
-    const {
-  return trajectory_data_;
-}
-
 transform::Rigid3d OptimizationProblem3D::ComputeRelativePose(
     const int trajectory_id, const NodeData& first_node_data,
     const NodeData& second_node_data) const {

cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h

@@ -28,6 +28,7 @@
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/id.h"
+#include "cartographer/mapping/internal/pose_graph/optimization_problem_interface.h"
 #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
 #include "cartographer/mapping/pose_graph_interface.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
@@ -40,80 +41,89 @@ namespace cartographer {
 namespace mapping {
 namespace pose_graph {
 
-// Implements the SPA loop closure method.
+struct NodeData3D {
-class OptimizationProblem3D {
- public:
-  using Constraint = PoseGraphInterface::Constraint;
-  using LandmarkNode = PoseGraphInterface::LandmarkNode;
-
-  struct NodeData {
   common::Time time;
   transform::Rigid3d local_pose;
   transform::Rigid3d global_pose;
 };
 
-  struct SubmapData {
+struct SubmapData3D {
   transform::Rigid3d global_pose;
 };
 
-  enum class FixZ { kYes, kNo };
+class OptimizationProblem3D
-
+    : public OptimizationProblemInterface<NodeData3D, SubmapData3D,
-  OptimizationProblem3D(
+                                          transform::Rigid3d> {
-      const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z);
+ public:
+  explicit OptimizationProblem3D(
+      const pose_graph::proto::OptimizationProblemOptions& options);
   ~OptimizationProblem3D();
 
   OptimizationProblem3D(const OptimizationProblem3D&) = delete;
   OptimizationProblem3D& operator=(const OptimizationProblem3D&) = delete;
 
-  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override;
   void AddOdometryData(int trajectory_id,
-                       const sensor::OdometryData& odometry_data);
+                       const sensor::OdometryData& odometry_data) override;
+  void AddTrajectoryNode(int trajectory_id,
+                         const NodeData3D& node_data) override;
+  void InsertTrajectoryNode(const NodeId& node_id,
+                            const NodeData3D& node_data) override;
+  void TrimTrajectoryNode(const NodeId& node_id) override;
+  void AddSubmap(int trajectory_id,
+                 const transform::Rigid3d& global_submap_pose) override;
+  void InsertSubmap(const SubmapId& submap_id,
+                    const transform::Rigid3d& global_submap_pose) override;
+  void TrimSubmap(const SubmapId& submap_id) override;
+  void SetMaxNumIterations(int32 max_num_iterations) override;
+
+  void Solve(
+      const std::vector<Constraint>& constraints,
+      const std::set<int>& frozen_trajectories,
+      const std::map<std::string, LandmarkNode>& landmark_nodes) override;
+
+  const MapById<NodeId, NodeData3D>& node_data() const override {
+    return node_data_;
+  }
+  const MapById<SubmapId, SubmapData3D>& submap_data() const override {
+    return submap_data_;
+  }
+  const std::map<std::string, transform::Rigid3d>& landmark_data()
+      const override {
+    return landmark_data_;
+  }
+  const sensor::MapByTime<sensor::ImuData>& imu_data() const override {
+    return imu_data_;
+  }
+  const sensor::MapByTime<sensor::OdometryData>& odometry_data()
+      const override {
+    return odometry_data_;
+  }
+
   void AddFixedFramePoseData(
       int trajectory_id,
       const sensor::FixedFramePoseData& fixed_frame_pose_data);
-  void AddTrajectoryNode(int trajectory_id, common::Time time,
-                         const transform::Rigid3d& local_pose,
-                         const transform::Rigid3d& global_pose);
   void SetTrajectoryData(
       int trajectory_id,
       const PoseGraphInterface::TrajectoryData& trajectory_data);
-  void InsertTrajectoryNode(const NodeId& node_id, common::Time time,
-                            const transform::Rigid3d& local_pose,
-                            const transform::Rigid3d& global_pose);
-  void TrimTrajectoryNode(const NodeId& node_id);
-  void AddSubmap(int trajectory_id,
-                 const transform::Rigid3d& global_submap_pose);
-  void InsertSubmap(const SubmapId& submap_id,
-                    const transform::Rigid3d& global_submap_pose);
-  void TrimSubmap(const SubmapId& submap_id);
-
-  void SetMaxNumIterations(int32 max_num_iterations);
-
-  // Optimizes the global poses.
-  void Solve(const std::vector<Constraint>& constraints,
-             const std::set<int>& frozen_trajectories,
-             const std::map<std::string, LandmarkNode>& landmark_nodes);
-
-  const MapById<NodeId, NodeData>& node_data() const;
-  const MapById<SubmapId, SubmapData>& submap_data() const;
-  const std::map<std::string, transform::Rigid3d>& landmark_data() const;
-  const sensor::MapByTime<sensor::ImuData>& imu_data() const;
-  const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
   const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
-      const;
+      const {
+    return fixed_frame_pose_data_;
+  }
   const std::map<int, PoseGraphInterface::TrajectoryData>& trajectory_data()
-      const;
+      const {
+    return trajectory_data_;
+  }
 
  private:
   // Uses odometry if available, otherwise the local SLAM results.
   transform::Rigid3d ComputeRelativePose(
-      int trajectory_id, const NodeData& first_node_data,
+      int trajectory_id, const NodeData3D& first_node_data,
-      const NodeData& second_node_data) const;
+      const NodeData3D& second_node_data) const;
 
   pose_graph::proto::OptimizationProblemOptions options_;
-  FixZ fix_z_;
+  MapById<NodeId, NodeData3D> node_data_;
-  MapById<NodeId, NodeData> node_data_;
+  MapById<SubmapId, SubmapData3D> submap_data_;
-  MapById<SubmapId, SubmapData> submap_data_;
   std::map<std::string, transform::Rigid3d> landmark_data_;
   sensor::MapByTime<sensor::ImuData> imu_data_;
   sensor::MapByTime<sensor::OdometryData> odometry_data_;

cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc

@@ -34,9 +34,7 @@ namespace {
 class OptimizationProblem3DTest : public ::testing::Test {
  protected:
   OptimizationProblem3DTest()
-      : optimization_problem_(CreateOptions(),
+      : optimization_problem_(CreateOptions()), rng_(45387) {}
-                              OptimizationProblem3D::FixZ::kNo),
-        rng_(45387) {}
 
   pose_graph::proto::OptimizationProblemOptions CreateOptions() {
     auto parameter_dictionary = common::MakeDictionary(R"text(
@@ -127,7 +125,8 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
     optimization_problem_.AddImuData(
         kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81,
                                        Eigen::Vector3d::Zero()});
-    optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose, pose);
+    optimization_problem_.AddTrajectoryNode(kTrajectoryId,
+                                            NodeData3D{now, pose, pose});
     now += common::FromSeconds(0.01);
   }
 
@@ -170,7 +169,7 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
-  const std::set<int> kFrozen;
+  const std::set<int> kFrozen = {};
   optimization_problem_.Solve(constraints, kFrozen, {});
 
   double translation_error_after = 0.;

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -42,8 +42,7 @@ namespace mapping {
 PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options,
                          common::ThreadPool* thread_pool)
     : options_(options),
-      optimization_problem_(options_.optimization_problem_options(),
+      optimization_problem_(options_.optimization_problem_options()),
-                            pose_graph::OptimizationProblem3D::FixZ::kNo),
       constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
 
 PoseGraph3D::~PoseGraph3D() {
@@ -264,7 +263,8 @@ void PoseGraph3D::ComputeConstraintsForNode(
       optimization_problem_.submap_data().at(matching_id).global_pose *
       insertion_submaps.front()->local_pose().inverse() * local_pose;
   optimization_problem_.AddTrajectoryNode(
-      matching_id.trajectory_id, constant_data->time, local_pose, global_pose);
+      matching_id.trajectory_id,
+      pose_graph::NodeData3D{constant_data->time, local_pose, global_pose});
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const SubmapId submap_id = submap_ids[i];
     // Even if this was the last node added to 'submap_id', the submap will only
@@ -456,9 +456,8 @@ void PoseGraph3D::AddSubmapFromProto(
   submap_data_.Insert(submap_id, SubmapData());
   submap_data_.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the 'global_submap_pose'.
-  global_submap_poses_.Insert(
+  global_submap_poses_.Insert(submap_id,
-      submap_id,
+                              pose_graph::SubmapData3D{global_submap_pose});
-      pose_graph::OptimizationProblem3D::SubmapData{global_submap_pose});
   AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
     submap_data_.at(submap_id).state = SubmapState::kFinished;
     optimization_problem_.InsertSubmap(submap_id, global_submap_pose);
@@ -479,7 +478,9 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
   AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
     const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
     optimization_problem_.InsertTrajectoryNode(
-        node_id, constant_data->time, constant_data->local_pose, global_pose);
+        node_id,
+        pose_graph::NodeData3D{constant_data->time, constant_data->local_pose,
+                               global_pose});
   });
 }
 
@@ -762,8 +763,7 @@ PoseGraph3D::GetAllSubmapPoses() {
 }
 
 transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
-    const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
+    const MapById<SubmapId, pose_graph::SubmapData3D>& global_submap_poses,
-        global_submap_poses,
     const int trajectory_id) const {
   auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
   auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -191,8 +191,7 @@ class PoseGraph3D : public PoseGraph {
   // Computes the local to global map frame transform based on the given
   // 'global_submap_poses'.
   transform::Rigid3d ComputeLocalToGlobalTransform(
-      const MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>&
+      const MapById<SubmapId, pose_graph::SubmapData3D>& global_submap_poses,
-          global_submap_poses,
       int trajectory_id) const REQUIRES(mutex_);
 
   PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
@@ -248,8 +247,8 @@ class PoseGraph3D : public PoseGraph {
   int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
 
   // Global submap poses currently used for displaying data.
-  MapById<SubmapId, pose_graph::OptimizationProblem3D::SubmapData>
+  MapById<SubmapId, pose_graph::SubmapData3D> global_submap_poses_
-      global_submap_poses_ GUARDED_BY(mutex_);
+      GUARDED_BY(mutex_);
 
   // Global landmark poses with all observations.
   std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>

cartographer/mapping/internal/pose_graph/optimization_problem_interface.h

@@ -0,0 +1,88 @@
+/*
+ * 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_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_
+
+#include <map>
+#include <set>
+#include <vector>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/common/time.h"
+#include "cartographer/mapping/id.h"
+#include "cartographer/mapping/pose_graph_interface.h"
+#include "cartographer/sensor/fixed_frame_pose_data.h"
+#include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/map_by_time.h"
+#include "cartographer/sensor/odometry_data.h"
+
+namespace cartographer {
+namespace mapping {
+namespace pose_graph {
+
+// Implements the SPA loop closure method.
+template <typename NodeDataType, typename SubmapDataType,
+          typename RigidTransformType>
+class OptimizationProblemInterface {
+ public:
+  using Constraint = PoseGraphInterface::Constraint;
+  using LandmarkNode = PoseGraphInterface::LandmarkNode;
+
+  OptimizationProblemInterface() {}
+  virtual ~OptimizationProblemInterface() {}
+
+  OptimizationProblemInterface(const OptimizationProblemInterface&) = delete;
+  OptimizationProblemInterface& operator=(const OptimizationProblemInterface&) =
+      delete;
+
+  virtual void AddImuData(int trajectory_id,
+                          const sensor::ImuData& imu_data) = 0;
+  virtual void AddOdometryData(int trajectory_id,
+                               const sensor::OdometryData& odometry_data) = 0;
+  virtual void AddTrajectoryNode(int trajectory_id,
+                                 const NodeDataType& node_data) = 0;
+  virtual void InsertTrajectoryNode(const NodeId& node_id,
+                                    const NodeDataType& node_data) = 0;
+  virtual void TrimTrajectoryNode(const NodeId& node_id) = 0;
+  virtual void AddSubmap(int trajectory_id,
+                         const RigidTransformType& global_submap_pose) = 0;
+  virtual void InsertSubmap(const SubmapId& submap_id,
+                            const RigidTransformType& global_submap_pose) = 0;
+  virtual void TrimSubmap(const SubmapId& submap_id) = 0;
+  virtual void SetMaxNumIterations(int32 max_num_iterations) = 0;
+
+  // Optimizes the global poses.
+  virtual void Solve(
+      const std::vector<Constraint>& constraints,
+      const std::set<int>& frozen_trajectories,
+      const std::map<std::string, LandmarkNode>& landmark_nodes) = 0;
+
+  virtual const MapById<NodeId, NodeDataType>& node_data() const = 0;
+  virtual const MapById<SubmapId, SubmapDataType>& submap_data() const = 0;
+  virtual const std::map<std::string, transform::Rigid3d>& landmark_data()
+      const = 0;
+  virtual const sensor::MapByTime<sensor::ImuData>& imu_data() const = 0;
+  virtual const sensor::MapByTime<sensor::OdometryData>& odometry_data()
+      const = 0;
+};
+
+}  // namespace pose_graph
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_

cartographer/mapping/pose_graph/proto/optimization_problem_options.proto

@@ -18,7 +18,7 @@ package cartographer.mapping.pose_graph.proto;
 
 import "cartographer/common/proto/ceres_solver_options.proto";
 
-// NEXT ID: 13
+// NEXT ID: 14
 message OptimizationProblemOptions {
   // Scaling parameter for Huber loss function.
   double huber_scale = 1;
@@ -41,6 +41,9 @@ message OptimizationProblemOptions {
   // Scaling parameter for the FixedFramePose rotation.
   double fixed_frame_pose_rotation_weight = 12;
 
+  // 3D only: fix Z.
+  bool fix_z_in_3d = 13;
+
   // If true, the Ceres solver summary will be logged for every optimization.
   bool log_solver_summary = 5;