Use (trajectory_id, submap_index) as submap IDs in places. (#271)

Replace flat submap index through (trajectory_id, submap_index) in Constraints.
Related to #256.

PAIR=wohe

cartographer/mapping/sparse_pose_graph.cc

@@ -53,22 +53,6 @@ void GroupTrajectoryNodes(
   }
 }
 
-// TODO(macmason): This function is very nearly a copy of GroupTrajectoryNodes.
-// Consider refactoring them to share an implementation.
-void GroupSubmapStates(
-    const std::vector<SparsePoseGraph::SubmapState>& submap_states,
-    const std::unordered_map<const Submaps*, int>& trajectory_ids,
-    std::vector<std::pair<int, int>>* new_indices) {
-  CHECK_NOTNULL(new_indices)->clear();
-  std::vector<int> submap_group_sizes(trajectory_ids.size(), 0);
-
-  for (const auto& submap_state : submap_states) {
-    const int id = trajectory_ids.at(submap_state.trajectory);
-    new_indices->emplace_back(id, submap_group_sizes[id]);
-    submap_group_sizes[id]++;
-  }
-}
-
 proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::SparsePoseGraphOptions options;
@@ -96,10 +80,6 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
   GroupTrajectoryNodes(GetTrajectoryNodes(), trajectory_ids(), &grouped_nodes,
                        &grouped_node_indices);
 
-  std::vector<std::pair<int, int>> grouped_submap_indices;
-  GroupSubmapStates(GetSubmapStates(), trajectory_ids(),
-                    &grouped_submap_indices);
-
   for (const auto& constraint : constraints()) {
     auto* const constraint_proto = proto.add_constraint();
     *constraint_proto->mutable_relative_pose() =
@@ -113,9 +93,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
         constraint.pose.sqrt_Lambda_ij;
 
     constraint_proto->mutable_submap_id()->set_trajectory_id(
-        grouped_submap_indices[constraint.i].first);
+        constraint.i.trajectory_id);
     constraint_proto->mutable_submap_id()->set_submap_index(
-        grouped_submap_indices[constraint.i].second);
+        constraint.i.submap_index);
 
     constraint_proto->mutable_scan_id()->set_trajectory_id(
         grouped_node_indices[constraint.j].first);

cartographer/mapping/sparse_pose_graph.h

@@ -58,8 +58,9 @@ class SparsePoseGraph {
       Eigen::Matrix<double, 6, 6> sqrt_Lambda_ij;
     };
 
-    // Submap index.
+    // TODO(hrapp): Rename to 'submap_id' and mention that the paper calls this
-    int i;
+    // 'i'.
+    mapping::SubmapId i;
 
     // Scan index.
     int j;
@@ -73,24 +74,6 @@ class SparsePoseGraph {
     enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
   };
 
-  struct SubmapState {
-    const Submap* submap = nullptr;
-
-    // Indices of the scans that were inserted into this map together with
-    // constraints for them. They are not to be matched again when this submap
-    // becomes 'finished'.
-    std::set<int> scan_indices;
-
-    // Whether in the current state of the background thread this submap is
-    // finished. When this transitions to true, all scans are tried to match
-    // against this submap. Likewise, all new scans are matched against submaps
-    // which are finished.
-    bool finished = false;
-
-    // The trajectory to which this SubmapState belongs.
-    const Submaps* trajectory = nullptr;
-  };
-
   SparsePoseGraph() {}
   virtual ~SparsePoseGraph() {}
 
@@ -121,10 +104,6 @@ class SparsePoseGraph {
   proto::SparsePoseGraph ToProto();
 
  protected:
-  // TODO(macmason, wohe): Consider replacing this with a GroupSubmapStates,
-  // which would have better separation of concerns.
-  virtual std::vector<SubmapState> GetSubmapStates() = 0;
-
   // Returns the collection of constraints.
   virtual std::vector<Constraint> constraints() = 0;
 
@@ -132,13 +111,6 @@ class SparsePoseGraph {
   virtual const std::unordered_map<const Submaps*, int>& trajectory_ids() = 0;
 };
 
-// Like TrajectoryNodes, SubmapStates arrive in a flat vector, but need to be
-// grouped by trajectory. The arguments are just as in 'GroupTrajectoryNodes'.
-void GroupSubmapStates(
-    const std::vector<SparsePoseGraph::SubmapState>& submap_states,
-    const std::unordered_map<const Submaps*, int>& trajectory_ids,
-    std::vector<std::pair<int, int>>* new_indices);
-
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer/mapping/submaps.h

@@ -63,6 +63,22 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
   return value;
 }
 
+// Uniquely identifies a submap using a combination of a unique trajectory ID
+// and a zero-based index of the submap inside that trajectory.
+struct SubmapId {
+  int trajectory_id;
+  int submap_index;
+
+  bool operator<(const SubmapId& other) const {
+    return std::forward_as_tuple(trajectory_id, submap_index) <
+           std::forward_as_tuple(other.trajectory_id, other.submap_index);
+  }
+};
+
+inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {
+  return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
+}
+
 // An individual submap, which has an initial position 'origin', keeps track of
 // how many range data were inserted into it, and sets the
 // 'finished_probability_grid' to be used for loop closing once the map no

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -64,7 +64,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
     // If we don't already have an entry for this submap, add one.
     if (first_submap_index == next_submap_index) {
       optimization_problem_.AddSubmap(
-          submap_states_[first_submap_index].trajectory,
+          submap_states_[first_submap_index].id.trajectory_id,
           transform::Rigid2d::Identity());
     }
     return;
@@ -78,7 +78,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
     const auto& first_submap_pose =
         optimization_problem_.submap_data().at(first_submap_index).pose;
     optimization_problem_.AddSubmap(
-        submap_states_[second_submap_index].trajectory,
+        submap_states_[second_submap_index].id.trajectory_id,
         first_submap_pose *
             sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])
                 .inverse() *
@@ -116,6 +116,9 @@ void SparsePoseGraph::AddScan(
     submap_states_.emplace_back();
     submap_states_.back().submap = insertion_submaps.back();
     submap_states_.back().trajectory = trajectory;
+    submap_states_.back().id = mapping::SubmapId{
+        trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]};
+    ++num_submaps_in_trajectory_[trajectory];
     CHECK_EQ(submap_states_.size(), submap_indices_.size());
   }
   const mapping::Submap* const finished_submap =
@@ -165,11 +168,14 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
       trajectory_nodes_[scan_index].constant_data->trajectory;
   const mapping::Submaps* const submap_trajectory =
       submap_states_[submap_index].trajectory;
+
+  const mapping::SubmapId submap_id = submap_states_[submap_index].id;
+
   // Only globally match against submaps not in this trajectory.
   if (scan_trajectory != submap_trajectory &&
       global_localization_samplers_[scan_trajectory]->Pulse()) {
     constraint_builder_.MaybeAddGlobalConstraint(
-        submap_index, submap_states_[submap_index].submap, scan_index,
+        submap_id, submap_states_[submap_index].submap, scan_index,
         scan_trajectory, submap_trajectory, &trajectory_connectivity_,
         &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
   } else {
@@ -181,7 +187,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
     if (scan_trajectory == submap_trajectory ||
         scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
-          submap_index, submap_states_[submap_index].submap, scan_index,
+          submap_id, submap_states_[submap_index].submap, scan_index,
           &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
           relative_pose);
     }
@@ -227,7 +233,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     constexpr double kFakePositionCovariance = 1e-6;
     constexpr double kFakeOrientationCovariance = 1e-6;
     constraints_.push_back(Constraint{
-        submap_index,
+        submap_states_[submap_index].id,
         scan_index,
         {transform::Embed3D(constraint_transform),
          common::ComputeSpdMatrixSqrtInverse(
@@ -387,11 +393,6 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
-std::vector<SparsePoseGraph::SubmapState> SparsePoseGraph::GetSubmapStates() {
-  common::MutexLocker locker(&mutex_);
-  return submap_states_;
-}
-
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   common::MutexLocker locker(&mutex_);
   return constraints_;

cartographer/mapping_2d/sparse_pose_graph.h

@@ -93,12 +93,31 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_);
 
  protected:
-  std::vector<SubmapState> GetSubmapStates() override EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
   const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
       override EXCLUDES(mutex_);
 
  private:
+  struct SubmapState {
+    const mapping::Submap* submap = nullptr;
+
+    // Indices of the scans that were inserted into this map together with
+    // constraints for them. They are not to be matched again when this submap
+    // becomes 'finished'.
+    std::set<int> scan_indices;
+
+    // Whether in the current state of the background thread this submap is
+    // finished. When this transitions to true, all scans are tried to match
+    // against this submap. Likewise, all new scans are matched against submaps
+    // which are finished.
+    bool finished = false;
+
+    // The trajectory to which this SubmapState belongs.
+    const mapping::Submaps* trajectory = nullptr;
+
+    mapping::SubmapId id;
+  };
+
   // Handles a new work item.
   void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
 
@@ -177,6 +196,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // before they take part in the background computations.
   std::map<const mapping::Submap*, int> submap_indices_ GUARDED_BY(mutex_);
   std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
+  std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
+      GUARDED_BY(mutex_);
 
   // Connectivity structure of our trajectories.
   std::vector<std::vector<const mapping::Submaps*>> connected_components_;

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -61,7 +61,7 @@ ConstraintBuilder::~ConstraintBuilder() {
 }
 
 void ConstraintBuilder::MaybeAddConstraint(
-    const int submap_index, const mapping::Submap* const submap,
+    const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
     const int scan_index, const sensor::PointCloud* const point_cloud,
     const transform::Rigid2d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
@@ -76,9 +76,8 @@ void ConstraintBuilder::MaybeAddConstraint(
     ++pending_computations_[current_computation_];
     const int current_computation = current_computation_;
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-        submap_index, submap->finished_probability_grid,
+        submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
-        [=]() EXCLUDES(mutex_) {
+          ComputeConstraint(submap_id, submap, scan_index,
-          ComputeConstraint(submap_index, submap, scan_index,
                             nullptr, /* scan_trajectory */
                             nullptr, /* submap_trajectory */
                             false,   /* match_full_submap */
@@ -90,7 +89,7 @@ void ConstraintBuilder::MaybeAddConstraint(
 }
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
-    const int submap_index, const mapping::Submap* const submap,
+    const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
     const int scan_index, const mapping::Submaps* scan_trajectory,
     const mapping::Submaps* submap_trajectory,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
@@ -102,8 +101,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   ++pending_computations_[current_computation_];
   const int current_computation = current_computation_;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-      submap_index, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
+      submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
-        ComputeConstraint(submap_index, submap, scan_index, submap_trajectory,
+        ComputeConstraint(submap_id, submap, scan_index, submap_trajectory,
                           scan_trajectory, true, /* match_full_submap */
                           trajectory_connectivity, point_cloud,
                           transform::Rigid2d::Identity(), constraint);
@@ -130,47 +129,45 @@ void ConstraintBuilder::WhenDone(
 }
 
 void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-    const int submap_index, const ProbabilityGrid* const submap,
+    const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap,
     const std::function<void()> work_item) {
-  if (submap_scan_matchers_[submap_index].fast_correlative_scan_matcher !=
+  if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
       nullptr) {
     thread_pool_->Schedule(work_item);
   } else {
-    submap_queued_work_items_[submap_index].push_back(work_item);
+    submap_queued_work_items_[submap_id].push_back(work_item);
-    if (submap_queued_work_items_[submap_index].size() == 1) {
+    if (submap_queued_work_items_[submap_id].size() == 1) {
       thread_pool_->Schedule(
-          std::bind(std::mem_fn(&ConstraintBuilder::ConstructSubmapScanMatcher),
+          [=]() { ConstructSubmapScanMatcher(submap_id, submap); });
-                    this, submap_index, submap));
     }
   }
 }
 
 void ConstraintBuilder::ConstructSubmapScanMatcher(
-    const int submap_index, const ProbabilityGrid* const submap) {
+    const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap) {
   auto submap_scan_matcher =
       common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
           *submap, options_.fast_correlative_scan_matcher_options());
   common::MutexLocker locker(&mutex_);
-  submap_scan_matchers_[submap_index] = {submap,
+  submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)};
-                                         std::move(submap_scan_matcher)};
   for (const std::function<void()>& work_item :
-       submap_queued_work_items_[submap_index]) {
+       submap_queued_work_items_[submap_id]) {
     thread_pool_->Schedule(work_item);
   }
-  submap_queued_work_items_.erase(submap_index);
+  submap_queued_work_items_.erase(submap_id);
 }
 
 const ConstraintBuilder::SubmapScanMatcher*
-ConstraintBuilder::GetSubmapScanMatcher(const int submap_index) {
+ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
   const SubmapScanMatcher* submap_scan_matcher =
-      &submap_scan_matchers_[submap_index];
+      &submap_scan_matchers_[submap_id];
   CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr);
   return submap_scan_matcher;
 }
 
 void ConstraintBuilder::ComputeConstraint(
-    const int submap_index, const mapping::Submap* const submap,
+    const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
     const int scan_index, const mapping::Submaps* scan_trajectory,
     const mapping::Submaps* submap_trajectory, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
@@ -180,7 +177,7 @@ void ConstraintBuilder::ComputeConstraint(
   const transform::Rigid2d initial_pose =
       ComputeSubmapPose(*submap) * initial_relative_pose;
   const SubmapScanMatcher* const submap_scan_matcher =
-      GetSubmapScanMatcher(submap_index);
+      GetSubmapScanMatcher(submap_id);
   const sensor::PointCloud filtered_point_cloud =
       adaptive_voxel_filter_.Filter(*point_cloud);
 
@@ -231,7 +228,7 @@ void ConstraintBuilder::ComputeConstraint(
   constexpr double kFakePositionCovariance = 1e-6;
   constexpr double kFakeOrientationCovariance = 1e-6;
   constraint->reset(new Constraint{
-      submap_index,
+      submap_id,
       scan_index,
       {transform::Embed3D(constraint_transform),
        common::ComputeSpdMatrixSqrtInverse(
@@ -245,7 +242,7 @@ void ConstraintBuilder::ComputeConstraint(
         initial_pose.inverse() * pose_estimate;
     std::ostringstream info;
     info << "Scan index " << scan_index << " with "
-         << filtered_point_cloud.size() << " points on submap " << submap_index
+         << filtered_point_cloud.size() << " points on submap " << submap_id
          << " differs by translation " << std::fixed << std::setprecision(2)
          << difference.translation().norm() << " rotation "
          << std::setprecision(3) << std::abs(difference.normalized_angle())

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -72,17 +72,18 @@ class ConstraintBuilder {
   ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_index', and the 'point_cloud' for 'scan_index'.
+  // 'submap_id', and the 'point_cloud' for 'scan_index'.
   // The 'initial_pose' is relative to the 'submap'.
   //
   // The pointees of 'submap' and 'point_cloud' must stay valid until all
   // computations are finished.
-  void MaybeAddConstraint(int submap_index, const mapping::Submap* submap,
+  void MaybeAddConstraint(const mapping::SubmapId& submap_id,
-                          int scan_index, const sensor::PointCloud* point_cloud,
+                          const mapping::Submap* submap, int scan_index,
+                          const sensor::PointCloud* point_cloud,
                           const transform::Rigid2d& initial_relative_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_index' and the 'point_cloud' for 'scan_index'. This performs
+  // 'submap_id' and the 'point_cloud' for 'scan_index'. This performs
   // full-submap matching.
   //
   // The scan at 'scan_index' should be from trajectory 'scan_trajectory', and
@@ -92,8 +93,8 @@ class ConstraintBuilder {
   // The pointees of 'submap' and 'point_cloud' must stay valid until all
   // computations are finished.
   void MaybeAddGlobalConstraint(
-      int submap_index, const mapping::Submap* submap, int scan_index,
+      const mapping::SubmapId& submap_id, const mapping::Submap* submap,
-      const mapping::Submaps* scan_trajectory,
+      int scan_index, const mapping::Submaps* scan_trajectory,
       const mapping::Submaps* submap_trajectory,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::PointCloud* point_cloud);
@@ -118,17 +119,17 @@ class ConstraintBuilder {
   // Either schedules the 'work_item', or if needed, schedules the scan matcher
   // construction and queues the 'work_item'.
   void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-      int submap_index, const ProbabilityGrid* submap,
+      const mapping::SubmapId& submap_id, const ProbabilityGrid* submap,
       std::function<void()> work_item) REQUIRES(mutex_);
 
   // Constructs the scan matcher for a 'submap', then schedules its work items.
-  void ConstructSubmapScanMatcher(int submap_index,
+  void ConstructSubmapScanMatcher(const mapping::SubmapId& submap_id,
                                   const ProbabilityGrid* submap)
       EXCLUDES(mutex_);
 
   // Returns the scan matcher for a submap, which has to exist.
-  const SubmapScanMatcher* GetSubmapScanMatcher(int submap_index)
+  const SubmapScanMatcher* GetSubmapScanMatcher(
-      EXCLUDES(mutex_);
+      const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
 
   // Runs in a background thread and does computations for an additional
   // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
@@ -137,8 +138,8 @@ class ConstraintBuilder {
   // 'trajectory_connectivity'.
   // As output, it may create a new Constraint in 'constraint'.
   void ComputeConstraint(
-      int submap_index, const mapping::Submap* submap, int scan_index,
+      const mapping::SubmapId& submap_id, const mapping::Submap* submap,
-      const mapping::Submaps* scan_trajectory,
+      int scan_index, const mapping::Submaps* scan_trajectory,
       const mapping::Submaps* submap_trajectory, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::PointCloud* point_cloud,
@@ -170,14 +171,15 @@ class ConstraintBuilder {
   // keep pointers valid when adding more entries.
   std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
 
-  // Map of already constructed scan matchers by 'submap_index'.
+  // Map of already constructed scan matchers by 'submap_id'.
-  std::map<int, SubmapScanMatcher> submap_scan_matchers_ GUARDED_BY(mutex_);
+  std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_
-
-  // Map by 'submap_index' of scan matchers under construction, and the work
-  // to do once construction is done.
-  std::map<int, std::vector<std::function<void()>>> submap_queued_work_items_
       GUARDED_BY(mutex_);
 
+  // Map by 'submap_id' of scan matchers under construction, and the work
+  // to do once construction is done.
+  std::map<mapping::SubmapId, std::vector<std::function<void()>>>
+      submap_queued_work_items_ GUARDED_BY(mutex_);
+
   common::FixedRatioSampler sampler_;
   const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_;
   scan_matching::CeresScanMatcher ceres_scan_matcher_;

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -75,9 +75,9 @@ void OptimizationProblem::AddTrajectoryNode(
       NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose});
 }
 
-void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
+void OptimizationProblem::AddSubmap(const int trajectory_id,
                                     const transform::Rigid2d& submap_pose) {
-  submap_data_.push_back(SubmapData{trajectory, submap_pose});
+  submap_data_.push_back(SubmapData{trajectory_id, submap_pose});
 }
 
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@@ -95,11 +95,18 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
   ceres::Problem problem(problem_options);
 
   // Set the starting point.
-  std::vector<std::array<double, 3>> C_submaps(submap_data_.size());
+  // TODO(hrapp): Move ceres data into SubmapData.
+  std::deque<std::deque<std::array<double, 3>>> C_submaps;
   std::vector<std::array<double, 3>> C_point_clouds(node_data_.size());
   for (size_t i = 0; i != submap_data_.size(); ++i) {
-    C_submaps[i] = FromPose(submap_data_[i].pose);
+    while (static_cast<int>(C_submaps.size()) <=
-    problem.AddParameterBlock(C_submaps[i].data(), 3);
+           submap_data_[i].trajectory_id) {
+      C_submaps.emplace_back();
+    }
+    C_submaps[submap_data_[i].trajectory_id].push_back(
+        FromPose(submap_data_[i].pose));
+    problem.AddParameterBlock(
+        C_submaps[submap_data_[i].trajectory_id].back().data(), 3);
   }
   for (size_t j = 0; j != node_data_.size(); ++j) {
     C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose);
@@ -107,12 +114,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
   }
 
   // Fix the pose of the first submap.
-  problem.SetParameterBlockConstant(C_submaps[0].data());
+  problem.SetParameterBlockConstant(
+      C_submaps[submap_data_[0].trajectory_id].front().data());
 
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
-    CHECK_GE(constraint.i, 0);
-    CHECK_LT(constraint.i, submap_data_.size());
     CHECK_GE(constraint.j, 0);
     CHECK_LT(constraint.j, node_data_.size());
     problem.AddResidualBlock(
@@ -122,7 +128,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
         constraint.tag == Constraint::INTER_SUBMAP
             ? new ceres::HuberLoss(options_.huber_scale())
             : nullptr,
-        C_submaps[constraint.i].data(), C_point_clouds[constraint.j].data());
+        C_submaps.at(constraint.i.trajectory_id)
+            .at(constraint.i.submap_index)
+            .data(),
+        C_point_clouds[constraint.j].data());
   }
 
   // Add penalties for changes between consecutive scans.
@@ -170,9 +179,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
   }
 
   // Store the result.
-  for (size_t i = 0; i != submap_data_.size(); ++i) {
+  for (auto& submap_data : submap_data_) {
-    submap_data_[i].pose = ToPose(C_submaps[i]);
+    submap_data.pose = ToPose(C_submaps[submap_data.trajectory_id].front());
+    C_submaps[submap_data.trajectory_id].pop_front();
   }
+
   for (size_t j = 0; j != node_data_.size(); ++j) {
     node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]);
   }

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -44,7 +44,7 @@ struct NodeData {
 
 struct SubmapData {
   // TODO(whess): Keep nodes per trajectory instead.
-  const mapping::Submaps* trajectory;
+  const int trajectory_id;
   transform::Rigid2d pose;
 };
 
@@ -67,8 +67,7 @@ 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,
+  void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
-                 const transform::Rigid2d& submap_pose);
 
   void SetMaxNumIterations(int32 max_num_iterations);
 

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -65,7 +65,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
     // If we don't already have an entry for this submap, add one.
     if (first_submap_index == next_submap_index) {
       optimization_problem_.AddSubmap(
-          submap_states_[first_submap_index].trajectory,
+          submap_states_[first_submap_index].id.trajectory_id,
           transform::Rigid3d::Identity());
     }
     return;
@@ -79,7 +79,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
     const auto& first_submap_pose =
         optimization_problem_.submap_data().at(first_submap_index).pose;
     optimization_problem_.AddSubmap(
-        submap_states_[second_submap_index].trajectory,
+        submap_states_[second_submap_index].id.trajectory_id,
         first_submap_pose * insertion_submaps[0]->local_pose().inverse() *
             insertion_submaps[1]->local_pose());
   }
@@ -95,6 +95,7 @@ void SparsePoseGraph::AddScan(
       GetLocalToGlobalTransform(trajectory) * pose);
 
   common::MutexLocker locker(&mutex_);
+  trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
   const int j = trajectory_nodes_.size();
   CHECK_LT(j, std::numeric_limits<int>::max());
 
@@ -112,6 +113,9 @@ void SparsePoseGraph::AddScan(
     submap_states_.emplace_back();
     submap_states_.back().submap = insertion_submaps.back();
     submap_states_.back().trajectory = trajectory;
+    submap_states_.back().id = mapping::SubmapId{
+        trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]};
+    ++num_submaps_in_trajectory_[trajectory];
     CHECK_EQ(submap_states_.size(), submap_indices_.size());
   }
   const Submap* const finished_submap =
@@ -164,11 +168,14 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
       trajectory_nodes_[scan_index].constant_data->trajectory;
   const mapping::Submaps* const submap_trajectory =
       submap_states_[submap_index].trajectory;
+
+  const mapping::SubmapId submap_id = submap_states_[submap_index].id;
+
   // Only globally match against submaps not in this trajectory.
   if (scan_trajectory != submap_trajectory &&
       global_localization_samplers_[scan_trajectory]->Pulse()) {
     constraint_builder_.MaybeAddGlobalConstraint(
-        submap_index, submap_states_[submap_index].submap, scan_index,
+        submap_id, submap_states_[submap_index].submap, scan_index,
         scan_trajectory, submap_trajectory, &trajectory_connectivity_,
         trajectory_nodes_);
   } else {
@@ -180,7 +187,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
     if (scan_trajectory == submap_trajectory ||
         scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
-          submap_index, submap_states_[submap_index].submap, scan_index,
+          submap_id, submap_states_[submap_index].submap, scan_index,
           trajectory_nodes_, relative_pose);
     }
   }
@@ -222,7 +229,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     const transform::Rigid3d constraint_transform =
         submap->local_pose().inverse() * pose;
     constraints_.push_back(
-        Constraint{submap_index,
+        Constraint{submap_states_[submap_index].id,
                    scan_index,
                    {constraint_transform,
                     common::ComputeSpdMatrixSqrtInverse(
@@ -379,21 +386,6 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
-std::vector<mapping::SparsePoseGraph::SubmapState>
-SparsePoseGraph::GetSubmapStates() {
-  std::vector<mapping::SparsePoseGraph::SubmapState> result;
-  common::MutexLocker locker(&mutex_);
-  for (const auto& submap_state : submap_states_) {
-    mapping::SparsePoseGraph::SubmapState entry;
-    entry.submap = submap_state.submap;
-    entry.scan_indices = submap_state.scan_indices;
-    entry.finished = submap_state.finished;
-    entry.trajectory = submap_state.trajectory;
-    result.push_back(entry);
-  }
-  return result;
-}
-
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   common::MutexLocker locker(&mutex_);
   return constraints_;

cartographer/mapping_3d/sparse_pose_graph.h

@@ -95,14 +95,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_);
 
  protected:
-  std::vector<SubmapState> GetSubmapStates() EXCLUDES(mutex_) override;
+  std::vector<Constraint> constraints() override EXCLUDES(mutex_);
-  std::vector<Constraint> constraints() EXCLUDES(mutex_) override;
   const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
-      EXCLUDES(mutex_) override;
+      override EXCLUDES(mutex_);
 
  private:
-  // This is 'mapping::SubmapState', but with the 3D versions of 'submap' and
-  // 'trajectory'.
   struct SubmapState {
     const Submap* submap = nullptr;
 
@@ -119,6 +116,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
     // The trajectory to which this SubmapState belongs.
     const Submaps* trajectory = nullptr;
+
+    mapping::SubmapId id;
   };
 
   // Handles a new work item.
@@ -197,6 +196,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // before they take part in the background computations.
   std::map<const mapping::Submap*, int> submap_indices_ GUARDED_BY(mutex_);
   std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
+  std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
+      GUARDED_BY(mutex_);
 
   // Connectivity structure of our trajectories.
   std::vector<std::vector<const mapping::Submaps*>> connected_components_;

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -71,13 +71,14 @@ ConstraintBuilder::ConstraintBuilder(
 ConstraintBuilder::~ConstraintBuilder() {
   common::MutexLocker locker(&mutex_);
   CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
-  CHECK(pending_computations_.empty());
+  CHECK_EQ(pending_computations_.size(), 0);
   CHECK_EQ(submap_queued_work_items_.size(), 0);
   CHECK(when_done_ == nullptr);
 }
 
 void ConstraintBuilder::MaybeAddConstraint(
-    const int submap_index, const Submap* const submap, const int scan_index,
+    const mapping::SubmapId& submap_id, const Submap* const submap,
+    const int scan_index,
     const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
     const transform::Rigid3d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
@@ -96,9 +97,9 @@ void ConstraintBuilder::MaybeAddConstraint(
     const auto* const point_cloud =
         &trajectory_nodes[scan_index].constant_data->range_data_3d.returns;
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-        submap_index, submap_nodes, &submap->high_resolution_hybrid_grid,
+        submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
         [=]() EXCLUDES(mutex_) {
-          ComputeConstraint(submap_index, submap, scan_index,
+          ComputeConstraint(submap_id, submap, scan_index,
                             nullptr, /* scan_trajectory */
                             nullptr, /* submap_trajectory */
                             false,   /* match_full_submap */
@@ -110,8 +111,8 @@ void ConstraintBuilder::MaybeAddConstraint(
 }
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
-    const int submap_index, const Submap* const submap, const int scan_index,
+    const mapping::SubmapId& submap_id, const Submap* const submap,
-    const mapping::Submaps* scan_trajectory,
+    const int scan_index, const mapping::Submaps* scan_trajectory,
     const mapping::Submaps* submap_trajectory,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
@@ -126,9 +127,9 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   const auto* const point_cloud =
       &trajectory_nodes[scan_index].constant_data->range_data_3d.returns;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-      submap_index, submap_nodes, &submap->high_resolution_hybrid_grid,
+      submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
       [=]() EXCLUDES(mutex_) {
-        ComputeConstraint(submap_index, submap, scan_index, submap_trajectory,
+        ComputeConstraint(submap_id, submap, scan_index, submap_trajectory,
                           scan_trajectory, true, /* match_full_submap */
                           trajectory_connectivity, point_cloud,
                           transform::Rigid3d::Identity(), constraint);
@@ -155,24 +156,24 @@ void ConstraintBuilder::WhenDone(
 }
 
 void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-    const int submap_index,
+    const mapping::SubmapId& submap_id,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
     const HybridGrid* const submap, const std::function<void()> work_item) {
-  if (submap_scan_matchers_[submap_index].fast_correlative_scan_matcher !=
+  if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
       nullptr) {
     thread_pool_->Schedule(work_item);
   } else {
-    submap_queued_work_items_[submap_index].push_back(work_item);
+    submap_queued_work_items_[submap_id].push_back(work_item);
-    if (submap_queued_work_items_[submap_index].size() == 1) {
+    if (submap_queued_work_items_[submap_id].size() == 1) {
-      thread_pool_->Schedule(
+      thread_pool_->Schedule([=]() {
-          std::bind(std::mem_fn(&ConstraintBuilder::ConstructSubmapScanMatcher),
+        ConstructSubmapScanMatcher(submap_id, submap_nodes, submap);
-                    this, submap_index, submap_nodes, submap));
+      });
     }
   }
 }
 
 void ConstraintBuilder::ConstructSubmapScanMatcher(
-    const int submap_index,
+    const mapping::SubmapId& submap_id,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
     const HybridGrid* const submap) {
   auto submap_scan_matcher =
@@ -180,27 +181,26 @@ void ConstraintBuilder::ConstructSubmapScanMatcher(
           *submap, submap_nodes,
           options_.fast_correlative_scan_matcher_options_3d());
   common::MutexLocker locker(&mutex_);
-  submap_scan_matchers_[submap_index] = {submap,
+  submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)};
-                                         std::move(submap_scan_matcher)};
   for (const std::function<void()>& work_item :
-       submap_queued_work_items_[submap_index]) {
+       submap_queued_work_items_[submap_id]) {
     thread_pool_->Schedule(work_item);
   }
-  submap_queued_work_items_.erase(submap_index);
+  submap_queued_work_items_.erase(submap_id);
 }
 
 const ConstraintBuilder::SubmapScanMatcher*
-ConstraintBuilder::GetSubmapScanMatcher(const int submap_index) {
+ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
   const SubmapScanMatcher* submap_scan_matcher =
-      &submap_scan_matchers_[submap_index];
+      &submap_scan_matchers_[submap_id];
   CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr);
   return submap_scan_matcher;
 }
 
 void ConstraintBuilder::ComputeConstraint(
-    const int submap_index, const Submap* const submap, const int scan_index,
+    const mapping::SubmapId& submap_id, const Submap* const submap,
-    const mapping::Submaps* scan_trajectory,
+    const int scan_index, const mapping::Submaps* scan_trajectory,
     const mapping::Submaps* submap_trajectory, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::CompressedPointCloud* const compressed_point_cloud,
@@ -209,7 +209,7 @@ void ConstraintBuilder::ComputeConstraint(
   const transform::Rigid3d initial_pose =
       submap->local_pose() * initial_relative_pose;
   const SubmapScanMatcher* const submap_scan_matcher =
-      GetSubmapScanMatcher(submap_index);
+      GetSubmapScanMatcher(submap_id);
   const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
   const sensor::PointCloud filtered_point_cloud =
       adaptive_voxel_filter_.Filter(point_cloud);
@@ -258,7 +258,7 @@ void ConstraintBuilder::ComputeConstraint(
   const transform::Rigid3d constraint_transform =
       submap->local_pose().inverse() * pose_estimate;
   constraint->reset(new OptimizationProblem::Constraint{
-      submap_index,
+      submap_id,
       scan_index,
       {constraint_transform,
        1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) *
@@ -270,7 +270,7 @@ void ConstraintBuilder::ComputeConstraint(
         initial_pose.inverse() * pose_estimate;
     std::ostringstream info;
     info << "Scan index " << scan_index << " with "
-         << filtered_point_cloud.size() << " points on submap " << submap_index
+         << filtered_point_cloud.size() << " points on submap " << submap_id
          << " differs by translation " << std::fixed << std::setprecision(2)
          << difference.translation().norm() << " rotation "
          << std::setprecision(3) << transform::GetAngle(difference)

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -68,18 +68,18 @@ class ConstraintBuilder {
   ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_index', and the 'range_data_3d.returns' in 'trajectory_nodes' for
+  // 'submap_id', and the 'range_data_3d.returns' in 'trajectory_nodes' for
   // 'scan_index'. The 'initial_relative_pose' is relative to the 'submap'.
   //
   // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
   // all computations are finished.
   void MaybeAddConstraint(
-      int submap_index, const Submap* submap, int scan_index,
+      const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
       const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
       const transform::Rigid3d& initial_relative_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_index' and the 'range_data_3d.returns' in 'trajectory_nodes' for
+  // 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for
   // 'scan_index'. This performs full-submap matching.
   //
   // The scan at 'scan_index' should be from trajectory 'scan_trajectory', and
@@ -89,7 +89,7 @@ class ConstraintBuilder {
   // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
   // all computations are finished.
   void MaybeAddGlobalConstraint(
-      int submap_index, const Submap* submap, int scan_index,
+      const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
       const mapping::Submaps* scan_trajectory,
       const mapping::Submaps* submap_trajectory,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
@@ -115,20 +115,20 @@ class ConstraintBuilder {
   // Either schedules the 'work_item', or if needed, schedules the scan matcher
   // construction and queues the 'work_item'.
   void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-      int submap_index,
+      const mapping::SubmapId& submap_id,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
       const HybridGrid* submap, std::function<void()> work_item)
       REQUIRES(mutex_);
 
   // Constructs the scan matcher for a 'submap', then schedules its work items.
   void ConstructSubmapScanMatcher(
-      int submap_index,
+      const mapping::SubmapId& submap_id,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
       const HybridGrid* submap) EXCLUDES(mutex_);
 
   // Returns the scan matcher for a submap, which has to exist.
-  const SubmapScanMatcher* GetSubmapScanMatcher(int submap_index)
+  const SubmapScanMatcher* GetSubmapScanMatcher(
-      EXCLUDES(mutex_);
+      const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
 
   // Runs in a background thread and does computations for an additional
   // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
@@ -137,7 +137,7 @@ class ConstraintBuilder {
   // 'trajectory_connectivity'.
   // As output, it may create a new Constraint in 'constraint'.
   void ComputeConstraint(
-      int submap_index, const Submap* submap, int scan_index,
+      const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
       const mapping::Submaps* scan_trajectory,
       const mapping::Submaps* submap_trajectory, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
@@ -170,14 +170,15 @@ class ConstraintBuilder {
   // keep pointers valid when adding more entries.
   std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
 
-  // Map of already constructed scan matchers by 'submap_index'.
+  // Map of already constructed scan matchers by 'submap_id'.
-  std::map<int, SubmapScanMatcher> submap_scan_matchers_ GUARDED_BY(mutex_);
+  std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_
-
-  // Map by 'submap_index' of scan matchers under construction, and the work
-  // to do once construction is done.
-  std::map<int, std::vector<std::function<void()>>> submap_queued_work_items_
       GUARDED_BY(mutex_);
 
+  // Map by 'submap_id' of scan matchers under construction, and the work
+  // to do once construction is done.
+  std::map<mapping::SubmapId, std::vector<std::function<void()>>>
+      submap_queued_work_items_ GUARDED_BY(mutex_);
+
   common::FixedRatioSampler sampler_;
   const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_;
   scan_matching::CeresScanMatcher ceres_scan_matcher_;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -91,9 +91,9 @@ void OptimizationProblem::AddTrajectoryNode(
   node_data_.push_back(NodeData{trajectory, time, point_cloud_pose});
 }
 
-void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
+void OptimizationProblem::AddSubmap(const int trajectory_id,
                                     const transform::Rigid3d& submap_pose) {
-  submap_data_.push_back(SubmapData{trajectory, submap_pose});
+  submap_data_.push_back(SubmapData{trajectory_id, submap_pose});
 }
 
 void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@@ -125,19 +125,23 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
   };
 
   // Set the starting point.
-  std::deque<CeresPose> C_submaps;
+  // TODO(hrapp): Move ceres data into SubmapData.
+  std::vector<std::deque<CeresPose>> C_submaps(nodes_per_trajectory.size());
+
   std::deque<CeresPose> C_point_clouds;
+
   // Tie the first submap to the origin.
   CHECK(!submap_data_.empty());
-  C_submaps.emplace_back(
+  C_submaps[submap_data_[0].trajectory_id].emplace_back(
       transform::Rigid3d::Identity(), translation_parameterization(),
       common::make_unique<ceres::AutoDiffLocalParameterization<
           ConstantYawQuaternionPlus, 4, 2>>(),
       &problem);
-  problem.SetParameterBlockConstant(C_submaps.back().translation());
+  problem.SetParameterBlockConstant(
+      C_submaps[submap_data_[0].trajectory_id].back().translation());
 
   for (size_t i = 1; i != submap_data_.size(); ++i) {
-    C_submaps.emplace_back(
+    C_submaps[submap_data_[i].trajectory_id].emplace_back(
         submap_data_[i].pose, translation_parameterization(),
         common::make_unique<ceres::QuaternionParameterization>(), &problem);
   }
@@ -149,8 +153,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
 
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
-    CHECK_GE(constraint.i, 0);
-    CHECK_LT(constraint.i, submap_data_.size());
     CHECK_GE(constraint.j, 0);
     CHECK_LT(constraint.j, node_data_.size());
     problem.AddResidualBlock(
@@ -160,8 +162,12 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
         constraint.tag == Constraint::INTER_SUBMAP
             ? new ceres::HuberLoss(options_.huber_scale())
             : nullptr,
-        C_submaps[constraint.i].rotation(),
+        C_submaps.at(constraint.i.trajectory_id)
-        C_submaps[constraint.i].translation(),
+            .at(constraint.i.submap_index)
+            .rotation(),
+        C_submaps.at(constraint.i.trajectory_id)
+            .at(constraint.i.submap_index)
+            .translation(),
         C_point_clouds[constraint.j].rotation(),
         C_point_clouds[constraint.j].translation());
   }
@@ -242,9 +248,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
   }
 
   // Store the result.
-  for (size_t i = 0; i != submap_data_.size(); ++i) {
+  for (auto& submap_data : submap_data_) {
-    submap_data_[i].pose = C_submaps[i].ToRigid();
+    submap_data.pose = C_submaps[submap_data.trajectory_id].front().ToRigid();
+    C_submaps[submap_data.trajectory_id].pop_front();
   }
+
   for (size_t j = 0; j != node_data_.size(); ++j) {
     node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid();
   }

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -44,7 +44,7 @@ struct NodeData {
 
 struct SubmapData {
   // TODO(whess): Keep nodes per trajectory instead.
-  const mapping::Submaps* trajectory;
+  const int trajectory_id;
   transform::Rigid3d pose;
 };
 
@@ -69,8 +69,7 @@ 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,
+  void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
-                 const transform::Rigid3d& submap_pose);
 
   void SetMaxNumIterations(int32 max_num_iterations);
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -107,6 +107,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation(
       Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()));
   const mapping::Submaps* const kTrajectory = nullptr;
+  const int kTrajectoryId = 0;
 
   struct NoisyNode {
     transform::Rigid3d ground_truth_pose;
@@ -132,13 +133,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   std::vector<OptimizationProblem::Constraint> constraints;
   for (int j = 0; j != kNumNodes; ++j) {
     constraints.push_back(OptimizationProblem::Constraint{
-        0, j,
+        mapping::SubmapId{0, 0}, j,
         OptimizationProblem::Constraint::Pose{
             AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
             Eigen::Matrix<double, 6, 6>::Identity()}});
     // We add an additional independent, but equally noisy observation.
     constraints.push_back(OptimizationProblem::Constraint{
-        1, j,
+        mapping::SubmapId{0, 1}, j,
         OptimizationProblem::Constraint::Pose{
             AddNoise(test_data[j].ground_truth_pose,
                      RandomYawOnlyTransform(0.2, 0.3)),
@@ -146,7 +147,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
     // We add very noisy data with high covariance (i.e. small Lambda) to verify
     // it is mostly ignored.
     constraints.push_back(OptimizationProblem::Constraint{
-        2, j,
+        mapping::SubmapId{0, 2}, j,
         OptimizationProblem::Constraint::Pose{
             kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
                 RandomTransform(1e3, 3.),
@@ -165,9 +166,9 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
                             node_data[j].point_cloud_pose);
   }
 
-  optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
+  optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
-  optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
+  optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
-  optimization_problem_.AddSubmap(kTrajectory, kSubmap2Transform);
+  optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
   optimization_problem_.Solve(constraints);
 
   double translation_error_after = 0.;