`submap_pose`s are now owned by the OptimizationProblem. (#269)

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -52,31 +52,37 @@ SparsePoseGraph::~SparsePoseGraph() {
 }
 
 void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
-    const std::vector<const mapping::Submap*>& submaps) {
+    const std::vector<const mapping::Submap*>& insertion_submaps) {
-  CHECK(!submaps.empty());
+  CHECK(!insertion_submaps.empty());
-  CHECK_LT(submap_transforms_.size(), std::numeric_limits<int>::max());
+  CHECK_LT(optimization_problem_.submap_data().size(),
-  const int next_transform_index = submap_transforms_.size();
+           std::numeric_limits<int>::max());
+  const int next_submap_index = optimization_problem_.submap_data().size();
   // Verify that we have an index for the first submap.
-  const int first_transform_index = GetSubmapIndex(submaps[0]);
+  const int first_submap_index = GetSubmapIndex(insertion_submaps[0]);
-  if (submaps.size() == 1) {
+  CHECK_LE(first_submap_index, next_submap_index);
+  if (insertion_submaps.size() == 1) {
     // If we don't already have an entry for this submap, add one.
-    if (first_transform_index == next_transform_index) {
+    if (first_submap_index == next_submap_index) {
-      submap_transforms_.push_back(transform::Rigid2d::Identity());
+      optimization_problem_.AddSubmap(
+          submap_states_[first_submap_index].trajectory,
+          transform::Rigid2d::Identity());
     }
     return;
   }
-  CHECK_EQ(2, submaps.size());
+  CHECK_EQ(2, insertion_submaps.size());
   // CHECK that we have a index for the second submap.
-  const int second_transform_index = GetSubmapIndex(submaps[1]);
+  const int second_submap_index = GetSubmapIndex(insertion_submaps[1]);
-  CHECK_LE(second_transform_index, next_transform_index);
+  CHECK_LE(second_submap_index, next_submap_index);
   // Extrapolate if necessary.
-  if (second_transform_index == next_transform_index) {
+  if (second_submap_index == next_submap_index) {
-    const auto& first_submap_transform =
+    const auto& first_submap_pose =
-        submap_transforms_[first_transform_index];
+        optimization_problem_.submap_data().at(first_submap_index).pose;
-    submap_transforms_.push_back(
+    optimization_problem_.AddSubmap(
-        first_submap_transform *
+        submap_states_[second_submap_index].trajectory,
-        sparse_pose_graph::ComputeSubmapPose(*submaps[0]).inverse() *
+        first_submap_pose *
-        sparse_pose_graph::ComputeSubmapPose(*submaps[1]));
+            sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])
+                .inverse() *
+            sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
   }
 }
 
@@ -151,7 +157,7 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
 void SparsePoseGraph::ComputeConstraint(const int scan_index,
                                         const int submap_index) {
   const transform::Rigid2d relative_pose =
-      submap_transforms_[submap_index].inverse() *
+      optimization_problem_.submap_data().at(submap_index).pose.inverse() *
       optimization_problem_.node_data().at(scan_index).point_cloud_pose;
 
   const mapping::Submaps* const scan_trajectory =
@@ -203,7 +209,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   GrowSubmapTransformsAsNeeded(insertion_submaps);
   const int matching_index = GetSubmapIndex(matching_submap);
   const transform::Rigid2d optimized_pose =
-      submap_transforms_[matching_index] *
+      optimization_problem_.submap_data().at(matching_index).pose *
       sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
   CHECK_EQ(scan_index, optimization_problem_.node_data().size());
   const mapping::TrajectoryNode::ConstantData* const scan_data =
@@ -330,42 +336,47 @@ void SparsePoseGraph::RunFinalOptimization() {
 }
 
 void SparsePoseGraph::RunOptimization() {
-  if (!submap_transforms_.empty()) {
+  if (optimization_problem_.submap_data().empty()) {
-    optimization_problem_.Solve(constraints_, &submap_transforms_);
+    return;
-    common::MutexLocker locker(&mutex_);
+  }
+  optimization_problem_.Solve(constraints_);
+  common::MutexLocker locker(&mutex_);
 
-    const auto& node_data = optimization_problem_.node_data();
+  const auto& node_data = optimization_problem_.node_data();
-    const size_t num_optimized_poses = node_data.size();
+  const size_t num_optimized_poses = node_data.size();
-    for (size_t i = 0; i != num_optimized_poses; ++i) {
+  for (size_t i = 0; i != num_optimized_poses; ++i) {
-      trajectory_nodes_[i].pose =
+    trajectory_nodes_[i].pose =
-          transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose));
+        transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose));
+  }
+  // Extrapolate all point cloud poses that were added later.
+  std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
+      extrapolation_transforms;
+  std::vector<transform::Rigid2d> submap_transforms;
+  for (const auto& submap_data : optimization_problem_.submap_data()) {
+    submap_transforms.push_back(submap_data.pose);
+  }
+  for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
+    const mapping::Submaps* trajectory =
+        trajectory_nodes_[i].constant_data->trajectory;
+    if (extrapolation_transforms.count(trajectory) == 0) {
+      const auto new_submap_transforms =
+          ExtrapolateSubmapTransforms(submap_transforms, trajectory);
+      const auto old_submap_transforms =
+          ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
+      CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
+      extrapolation_transforms[trajectory] =
+          transform::Rigid3d(new_submap_transforms.back() *
+                             old_submap_transforms.back().inverse());
     }
-    // Extrapolate all point cloud poses that were added later.
+    trajectory_nodes_[i].pose =
-    std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
+        extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
-        extrapolation_transforms;
+  }
-    for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
+  optimized_submap_transforms_ = submap_transforms;
-      const mapping::Submaps* trajectory =
+  connected_components_ = trajectory_connectivity_.ConnectedComponents();
-          trajectory_nodes_[i].constant_data->trajectory;
+  reverse_connected_components_.clear();
-      if (extrapolation_transforms.count(trajectory) == 0) {
+  for (size_t i = 0; i != connected_components_.size(); ++i) {
-        const auto new_submap_transforms =
+    for (const auto* trajectory : connected_components_[i]) {
-            ExtrapolateSubmapTransforms(submap_transforms_, trajectory);
+      reverse_connected_components_.emplace(trajectory, i);
-        const auto old_submap_transforms = ExtrapolateSubmapTransforms(
-            optimized_submap_transforms_, trajectory);
-        CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
-        extrapolation_transforms[trajectory] =
-            transform::Rigid3d(new_submap_transforms.back() *
-                               old_submap_transforms.back().inverse());
-      }
-      trajectory_nodes_[i].pose =
-          extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
-    }
-    optimized_submap_transforms_ = submap_transforms_;
-    connected_components_ = trajectory_connectivity_.ConnectedComponents();
-    reverse_connected_components_.clear();
-    for (size_t i = 0; i != connected_components_.size(); ++i) {
-      for (const auto* trajectory : connected_components_[i]) {
-        reverse_connected_components_.emplace(trajectory, i);
-      }
     }
   }
 }

cartographer/mapping_2d/sparse_pose_graph.h

@@ -105,9 +105,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
     return iterator->second;
   }
 
-  // Grows 'submap_transforms_' to have an entry for every element of 'submaps'.
+  // Grows the optimization problem to have an entry for every element of
+  // 'insertion_submaps'.
   void GrowSubmapTransformsAsNeeded(
-      const std::vector<const mapping::Submap*>& submaps) REQUIRES(mutex_);
+      const std::vector<const mapping::Submap*>& insertion_submaps)
+      REQUIRES(mutex_);
 
   // Adds constraints for a scan, and starts scan matching in the background.
   void ComputeConstraintsForScan(
@@ -167,7 +169,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   sparse_pose_graph::OptimizationProblem optimization_problem_;
   sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
   std::vector<Constraint> constraints_;
-  std::vector<transform::Rigid2d> submap_transforms_;  // (map <- submap)
 
   // Submaps get assigned an index and state as soon as they are seen, even
   // before they take part in the background computations.

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -75,14 +75,17 @@ void OptimizationProblem::AddTrajectoryNode(
       NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose});
 }
 
+void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
+                                    const transform::Rigid2d& submap_pose) {
+  submap_data_.push_back(SubmapData{trajectory, submap_pose});
+}
+
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
   options_.mutable_ceres_solver_options()->set_max_num_iterations(
       max_num_iterations);
 }
 
-void OptimizationProblem::Solve(
+void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
-    const std::vector<Constraint>& constraints,
-    std::vector<transform::Rigid2d>* const submap_transforms) {
   if (node_data_.empty()) {
     // Nothing to optimize.
     return;
@@ -92,10 +95,10 @@ void OptimizationProblem::Solve(
   ceres::Problem problem(problem_options);
 
   // Set the starting point.
-  std::vector<std::array<double, 3>> C_submaps(submap_transforms->size());
+  std::vector<std::array<double, 3>> C_submaps(submap_data_.size());
   std::vector<std::array<double, 3>> C_point_clouds(node_data_.size());
-  for (size_t i = 0; i != submap_transforms->size(); ++i) {
+  for (size_t i = 0; i != submap_data_.size(); ++i) {
-    C_submaps[i] = FromPose((*submap_transforms)[i]);
+    C_submaps[i] = FromPose(submap_data_[i].pose);
     problem.AddParameterBlock(C_submaps[i].data(), 3);
   }
   for (size_t j = 0; j != node_data_.size(); ++j) {
@@ -109,7 +112,7 @@ void OptimizationProblem::Solve(
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
     CHECK_GE(constraint.i, 0);
-    CHECK_LT(constraint.i, submap_transforms->size());
+    CHECK_LT(constraint.i, submap_data_.size());
     CHECK_GE(constraint.j, 0);
     CHECK_LT(constraint.j, node_data_.size());
     problem.AddResidualBlock(
@@ -167,8 +170,8 @@ void OptimizationProblem::Solve(
   }
 
   // Store the result.
-  for (size_t i = 0; i != submap_transforms->size(); ++i) {
+  for (size_t i = 0; i != submap_data_.size(); ++i) {
-    (*submap_transforms)[i] = ToPose(C_submaps[i]);
+    submap_data_[i].pose = ToPose(C_submaps[i]);
   }
   for (size_t j = 0; j != node_data_.size(); ++j) {
     node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]);
@@ -179,6 +182,10 @@ const std::vector<NodeData>& OptimizationProblem::node_data() const {
   return node_data_;
 }
 
+const std::vector<SubmapData>& OptimizationProblem::submap_data() const {
+  return submap_data_;
+}
+
 }  // namespace sparse_pose_graph
 }  // namespace mapping_2d
 }  // namespace cartographer

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -42,6 +42,12 @@ struct NodeData {
   transform::Rigid2d point_cloud_pose;
 };
 
+struct SubmapData {
+  // TODO(whess): Keep nodes per trajectory instead.
+  const mapping::Submaps* trajectory;
+  transform::Rigid2d pose;
+};
+
 // Implements the SPA loop closure method.
 class OptimizationProblem {
  public:
@@ -61,19 +67,22 @@ class OptimizationProblem {
   void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
                          const transform::Rigid2d& initial_point_cloud_pose,
                          const transform::Rigid2d& point_cloud_pose);
+  void AddSubmap(const mapping::Submaps* trajectory,
+                 const transform::Rigid2d& submap_pose);
 
   void SetMaxNumIterations(int32 max_num_iterations);
 
   // Computes the optimized poses.
-  void Solve(const std::vector<Constraint>& constraints,
+  void Solve(const std::vector<Constraint>& constraints);
-             std::vector<transform::Rigid2d>* submap_transforms);
 
   const std::vector<NodeData>& node_data() const;
+  const std::vector<SubmapData>& submap_data() const;
 
  private:
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   std::map<const mapping::Submaps*, std::deque<mapping_3d::ImuData>> imu_data_;
   std::vector<NodeData> node_data_;
+  std::vector<SubmapData> submap_data_;
 };
 
 }  // namespace sparse_pose_graph

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -53,30 +53,35 @@ SparsePoseGraph::~SparsePoseGraph() {
 }
 
 void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
-    const std::vector<const Submap*>& submaps) {
+    const std::vector<const Submap*>& insertion_submaps) {
-  CHECK(!submaps.empty());
+  CHECK(!insertion_submaps.empty());
-  CHECK_LT(submap_transforms_.size(), std::numeric_limits<int>::max());
+  CHECK_LT(optimization_problem_.submap_data().size(),
-  const int next_transform_index = submap_transforms_.size();
+           std::numeric_limits<int>::max());
+  const int next_submap_index = optimization_problem_.submap_data().size();
   // Verify that we have an index for the first submap.
-  const int first_transform_index = GetSubmapIndex(submaps[0]);
+  const int first_submap_index = GetSubmapIndex(insertion_submaps[0]);
-  if (submaps.size() == 1) {
+  CHECK_LE(first_submap_index, next_submap_index);
+  if (insertion_submaps.size() == 1) {
     // If we don't already have an entry for this submap, add one.
-    if (first_transform_index == next_transform_index) {
+    if (first_submap_index == next_submap_index) {
-      submap_transforms_.push_back(transform::Rigid3d::Identity());
+      optimization_problem_.AddSubmap(
+          submap_states_[first_submap_index].trajectory,
+          transform::Rigid3d::Identity());
     }
     return;
   }
-  CHECK_EQ(2, submaps.size());
+  CHECK_EQ(2, insertion_submaps.size());
   // CHECK that we have a index for the second submap.
-  const int second_transform_index = GetSubmapIndex(submaps[1]);
+  const int second_submap_index = GetSubmapIndex(insertion_submaps[1]);
-  CHECK_LE(second_transform_index, next_transform_index);
+  CHECK_LE(second_submap_index, next_submap_index);
   // Extrapolate if necessary.
-  if (second_transform_index == next_transform_index) {
+  if (second_submap_index == next_submap_index) {
-    const auto& first_submap_transform =
+    const auto& first_submap_pose =
-        submap_transforms_[first_transform_index];
+        optimization_problem_.submap_data().at(first_submap_index).pose;
-    submap_transforms_.push_back(first_submap_transform *
+    optimization_problem_.AddSubmap(
-                                 submaps[0]->local_pose().inverse() *
+        submap_states_[second_submap_index].trajectory,
-                                 submaps[1]->local_pose());
+        first_submap_pose * insertion_submaps[0]->local_pose().inverse() *
+            insertion_submaps[1]->local_pose());
   }
 }
 
@@ -152,7 +157,7 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
 void SparsePoseGraph::ComputeConstraint(const int scan_index,
                                         const int submap_index) {
   const transform::Rigid3d relative_pose =
-      submap_transforms_[submap_index].inverse() *
+      optimization_problem_.submap_data().at(submap_index).pose.inverse() *
       optimization_problem_.node_data().at(scan_index).point_cloud_pose;
 
   const mapping::Submaps* const scan_trajectory =
@@ -202,7 +207,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   GrowSubmapTransformsAsNeeded(insertion_submaps);
   const int matching_index = GetSubmapIndex(matching_submap);
   const transform::Rigid3d optimized_pose =
-      submap_transforms_[matching_index] *
+      optimization_problem_.submap_data().at(matching_index).pose *
       matching_submap->local_pose().inverse() * pose;
   CHECK_EQ(scan_index, optimization_problem_.node_data().size());
   const mapping::TrajectoryNode::ConstantData* const scan_data =
@@ -325,41 +330,46 @@ void SparsePoseGraph::RunFinalOptimization() {
 }
 
 void SparsePoseGraph::RunOptimization() {
-  if (!submap_transforms_.empty()) {
+  if (optimization_problem_.submap_data().empty()) {
-    optimization_problem_.Solve(constraints_, &submap_transforms_);
+    return;
-    common::MutexLocker locker(&mutex_);
+  }
+  optimization_problem_.Solve(constraints_);
+  common::MutexLocker locker(&mutex_);
 
-    const auto& node_data = optimization_problem_.node_data();
+  const auto& node_data = optimization_problem_.node_data();
-    const size_t num_optimized_poses = node_data.size();
+  const size_t num_optimized_poses = node_data.size();
-    for (size_t i = 0; i != num_optimized_poses; ++i) {
+  for (size_t i = 0; i != num_optimized_poses; ++i) {
-      trajectory_nodes_[i].pose = node_data[i].point_cloud_pose;
+    trajectory_nodes_[i].pose = node_data[i].point_cloud_pose;
+  }
+  // Extrapolate all point cloud poses that were added later.
+  std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
+      extrapolation_transforms;
+  std::vector<transform::Rigid3d> submap_transforms;
+  for (const auto& submap_data : optimization_problem_.submap_data()) {
+    submap_transforms.push_back(submap_data.pose);
+  }
+  for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
+    const mapping::Submaps* trajectory =
+        trajectory_nodes_[i].constant_data->trajectory;
+    if (extrapolation_transforms.count(trajectory) == 0) {
+      const auto new_submap_transforms =
+          ExtrapolateSubmapTransforms(submap_transforms, trajectory);
+      const auto old_submap_transforms =
+          ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
+      CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
+      extrapolation_transforms[trajectory] =
+          transform::Rigid3d(new_submap_transforms.back() *
+                             old_submap_transforms.back().inverse());
     }
-    // Extrapolate all point cloud poses that were added later.
+    trajectory_nodes_[i].pose =
-    std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
+        extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
-        extrapolation_transforms;
+  }
-    for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
+  optimized_submap_transforms_ = submap_transforms;
-      const mapping::Submaps* trajectory =
+  connected_components_ = trajectory_connectivity_.ConnectedComponents();
-          trajectory_nodes_[i].constant_data->trajectory;
+  reverse_connected_components_.clear();
-      if (extrapolation_transforms.count(trajectory) == 0) {
+  for (size_t i = 0; i != connected_components_.size(); ++i) {
-        const auto new_submap_transforms =
+    for (const auto* trajectory : connected_components_[i]) {
-            ExtrapolateSubmapTransforms(submap_transforms_, trajectory);
+      reverse_connected_components_.emplace(trajectory, i);
-        const auto old_submap_transforms = ExtrapolateSubmapTransforms(
-            optimized_submap_transforms_, trajectory);
-        CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
-        extrapolation_transforms[trajectory] =
-            transform::Rigid3d(new_submap_transforms.back() *
-                               old_submap_transforms.back().inverse());
-      }
-      trajectory_nodes_[i].pose =
-          extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
-    }
-    optimized_submap_transforms_ = submap_transforms_;
-    connected_components_ = trajectory_connectivity_.ConnectedComponents();
-    reverse_connected_components_.clear();
-    for (size_t i = 0; i != connected_components_.size(); ++i) {
-      for (const auto* trajectory : connected_components_[i]) {
-        reverse_connected_components_.emplace(trajectory, i);
-      }
     }
   }
 }

cartographer/mapping_3d/sparse_pose_graph.h

@@ -126,9 +126,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
     return iterator->second;
   }
 
-  // Grows 'submap_transforms_' to have an entry for every element of 'submaps'.
+  // Grows the optimization problem to have an entry for every element of
-  void GrowSubmapTransformsAsNeeded(const std::vector<const Submap*>& submaps)
+  // 'insertion_submaps'.
-      REQUIRES(mutex_);
+  void GrowSubmapTransformsAsNeeded(
+      const std::vector<const Submap*>& insertion_submaps) REQUIRES(mutex_);
 
   // Adds constraints for a scan, and starts scan matching in the background.
   void ComputeConstraintsForScan(
@@ -187,7 +188,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   sparse_pose_graph::OptimizationProblem optimization_problem_;
   sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
   std::vector<Constraint> constraints_;
-  std::vector<transform::Rigid3d> submap_transforms_;  // (map <- submap)
 
   // Submaps get assigned an index and state as soon as they are seen, even
   // before they take part in the background computations.

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -91,14 +91,17 @@ void OptimizationProblem::AddTrajectoryNode(
   node_data_.push_back(NodeData{trajectory, time, point_cloud_pose});
 }
 
+void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
+                                    const transform::Rigid3d& submap_pose) {
+  submap_data_.push_back(SubmapData{trajectory, submap_pose});
+}
+
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
   options_.mutable_ceres_solver_options()->set_max_num_iterations(
       max_num_iterations);
 }
 
-void OptimizationProblem::Solve(
+void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
-    const std::vector<Constraint>& constraints,
-    std::vector<transform::Rigid3d>* const submap_transforms) {
   if (node_data_.empty()) {
     // Nothing to optimize.
     return;
@@ -125,7 +128,7 @@ void OptimizationProblem::Solve(
   std::deque<CeresPose> C_submaps;
   std::deque<CeresPose> C_point_clouds;
   // Tie the first submap to the origin.
-  CHECK(!submap_transforms->empty());
+  CHECK(!submap_data_.empty());
   C_submaps.emplace_back(
       transform::Rigid3d::Identity(), translation_parameterization(),
       common::make_unique<ceres::AutoDiffLocalParameterization<
@@ -133,9 +136,9 @@ void OptimizationProblem::Solve(
       &problem);
   problem.SetParameterBlockConstant(C_submaps.back().translation());
 
-  for (size_t i = 1; i != submap_transforms->size(); ++i) {
+  for (size_t i = 1; i != submap_data_.size(); ++i) {
     C_submaps.emplace_back(
-        (*submap_transforms)[i], translation_parameterization(),
+        submap_data_[i].pose, translation_parameterization(),
         common::make_unique<ceres::QuaternionParameterization>(), &problem);
   }
   for (size_t j = 0; j != node_data_.size(); ++j) {
@@ -147,7 +150,7 @@ void OptimizationProblem::Solve(
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
     CHECK_GE(constraint.i, 0);
-    CHECK_LT(constraint.i, submap_transforms->size());
+    CHECK_LT(constraint.i, submap_data_.size());
     CHECK_GE(constraint.j, 0);
     CHECK_LT(constraint.j, node_data_.size());
     problem.AddResidualBlock(
@@ -239,8 +242,8 @@ void OptimizationProblem::Solve(
   }
 
   // Store the result.
-  for (size_t i = 0; i != submap_transforms->size(); ++i) {
+  for (size_t i = 0; i != submap_data_.size(); ++i) {
-    (*submap_transforms)[i] = C_submaps[i].ToRigid();
+    submap_data_[i].pose = C_submaps[i].ToRigid();
   }
   for (size_t j = 0; j != node_data_.size(); ++j) {
     node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid();
@@ -251,6 +254,10 @@ const std::vector<NodeData>& OptimizationProblem::node_data() const {
   return node_data_;
 }
 
+const std::vector<SubmapData>& OptimizationProblem::submap_data() const {
+  return submap_data_;
+}
+
 }  // namespace sparse_pose_graph
 }  // namespace mapping_3d
 }  // namespace cartographer

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -42,6 +42,12 @@ struct NodeData {
   transform::Rigid3d point_cloud_pose;
 };
 
+struct SubmapData {
+  // TODO(whess): Keep nodes per trajectory instead.
+  const mapping::Submaps* trajectory;
+  transform::Rigid3d pose;
+};
+
 // Implements the SPA loop closure method.
 class OptimizationProblem {
  public:
@@ -63,20 +69,23 @@ class OptimizationProblem {
                   const Eigen::Vector3d& angular_velocity);
   void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
                          const transform::Rigid3d& point_cloud_pose);
+  void AddSubmap(const mapping::Submaps* trajectory,
+                 const transform::Rigid3d& submap_pose);
 
   void SetMaxNumIterations(int32 max_num_iterations);
 
   // Computes the optimized poses.
-  void Solve(const std::vector<Constraint>& constraints,
+  void Solve(const std::vector<Constraint>& constraints);
-             std::vector<transform::Rigid3d>* submap_transforms);
 
   const std::vector<NodeData>& node_data() const;
+  const std::vector<SubmapData>& submap_data() const;
 
  private:
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   FixZ fix_z_;
   std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_;
   std::vector<NodeData> node_data_;
+  std::vector<SubmapData> submap_data_;
   double gravity_constant_ = 9.8;
 };
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -153,9 +153,6 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
             1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});
   }
 
-  std::vector<transform::Rigid3d> submap_transforms = {
-      kSubmap0Transform, kSubmap0Transform, kSubmap2Transform};
-
   double translation_error_before = 0.;
   double rotation_error_before = 0.;
   const auto& node_data = optimization_problem_.node_data();
@@ -168,7 +165,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
                             node_data[j].point_cloud_pose);
   }
 
-  optimization_problem_.Solve(constraints, &submap_transforms);
+  optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
+  optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
+  optimization_problem_.AddSubmap(kTrajectory, kSubmap2Transform);
+  optimization_problem_.Solve(constraints);
 
   double translation_error_after = 0.;
   double rotation_error_after = 0.;