Support extrapolating a single submap transform. (#338)

cartographer/mapping/map_builder.cc

@@ -113,20 +113,20 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
            " trajectories.";
   }
 
-  const std::vector<transform::Rigid3d> submap_transforms =
+  const int num_submaps = sparse_pose_graph_->num_submaps(trajectory_id);
-      sparse_pose_graph_->GetSubmapTransforms(trajectory_id);
+  if (submap_index < 0 || submap_index >= num_submaps) {
-  if (submap_index < 0 ||
-      static_cast<size_t>(submap_index) >= submap_transforms.size()) {
     return "Requested submap " + std::to_string(submap_index) +
            " from trajectory " + std::to_string(trajectory_id) +
-           " but there are only " + std::to_string(submap_transforms.size()) +
+           " but there are only " + std::to_string(num_submaps) +
            " submaps in this trajectory.";
   }
 
   const Submap* const submap =
       trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index);
   response->set_submap_version(submap->num_range_data());
-  submap->ToResponseProto(submap_transforms[submap_index], response);
+  const auto submap_pose = sparse_pose_graph_->GetSubmapTransform(
+      SubmapId{trajectory_id, submap_index});
+  submap->ToResponseProto(submap_pose, response);
   return "";
 }
 

cartographer/mapping/sparse_pose_graph.cc

@@ -86,9 +86,12 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
     }
 
     if (!single_trajectory_nodes.empty()) {
-      for (const auto& transform : GetSubmapTransforms(trajectory_id)) {
+      const int num_submaps_in_trajectory = num_submaps(trajectory_id);
+      for (int submap_index = 0; submap_index != num_submaps_in_trajectory;
+           ++submap_index) {
+        const SubmapId submap_id{static_cast<int>(trajectory_id), submap_index};
         *trajectory_proto->add_submap()->mutable_pose() =
-            transform::ToProto(transform);
+            transform::ToProto(GetSubmapTransform(submap_id));
       }
     }
   }

cartographer/mapping/sparse_pose_graph.h

@@ -49,8 +49,8 @@ class SparsePoseGraph {
       double rotation_weight;
     };
 
-    mapping::SubmapId submap_id;  // 'i' in the paper.
+    SubmapId submap_id;  // 'i' in the paper.
-    mapping::NodeId node_id;      // 'j' in the paper.
+    NodeId node_id;      // 'j' in the paper.
 
     // Pose of the scan 'j' relative to submap 'i'.
     Pose pose;
@@ -77,9 +77,11 @@ class SparsePoseGraph {
   // Gets the current trajectory clusters.
   virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
 
-  // Returns the current optimized transforms for the given 'trajectory_id'.
+  // Return the number of submaps for the given 'trajectory_id'.
-  virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
+  virtual int num_submaps(int trajectory_id) = 0;
-      int trajectory_id) = 0;
+
+  // Returns the current optimized transform for the given 'submap_id'.
+  virtual transform::Rigid3d GetSubmapTransform(const SubmapId& submap_id) = 0;
 
   // Returns the transform converting data in the local map frame (i.e. the
   // continuous, non-loop-closed frame) into the global map frame (i.e. the

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -427,41 +427,32 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
   return connected_components_;
 }
 
-std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
+int SparsePoseGraph::num_submaps(const int trajectory_id) {
-    const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   if (trajectory_id >= submap_data_.num_trajectories()) {
-    return {transform::Rigid3d::Identity()};
+    return 0;
   }
+  return submap_data_.num_indices(trajectory_id);
+}
 
-  // Submaps for which we have optimized poses.
+transform::Rigid3d SparsePoseGraph::GetSubmapTransform(
-  std::vector<transform::Rigid3d> result;
+    const mapping::SubmapId& submap_id) {
-  for (int submap_index = 0;
+  common::MutexLocker locker(&mutex_);
-       submap_index != submap_data_.num_indices(trajectory_id);
+  // We already have an optimized pose.
-       ++submap_index) {
+  if (submap_id.trajectory_id <
-    const mapping::SubmapId submap_id{trajectory_id, submap_index};
+          static_cast<int>(optimized_submap_transforms_.size()) &&
-    const auto& submap_data = submap_data_.at(submap_id);
+      submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
-    if (static_cast<size_t>(trajectory_id) <
+                                                    .at(submap_id.trajectory_id)
-            optimized_submap_transforms_.size() &&
+                                                    .size())) {
-        result.size() < optimized_submap_transforms_.at(trajectory_id).size()) {
+    return transform::Embed3D(
-      // Submaps for which we have optimized poses.
+        optimized_submap_transforms_.at(submap_id.trajectory_id)
-      result.push_back(
+            .at(submap_id.submap_index)
-          transform::Embed3D(optimized_submap_transforms_.at(trajectory_id)
+            .pose);
-                                 .at(result.size())
-                                 .pose));
-    } else {
-      // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submap
-      // is okay, since the member is const.
-      result.push_back(ComputeLocalToGlobalTransform(
-                           optimized_submap_transforms_, trajectory_id) *
-                       submap_data.submap->local_pose());
-    }
   }
-
+  // We have to extrapolate.
-  if (result.empty()) {
+  return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
-    result.push_back(transform::Rigid3d::Identity());
+                                       submap_id.trajectory_id) *
-  }
+         submap_data_.at(submap_id).submap->local_pose();
-  return result;
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(

cartographer/mapping_2d/sparse_pose_graph.h

@@ -82,7 +82,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
-  std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
+  int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
+  transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id)
       EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -430,40 +430,31 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
   return connected_components_;
 }
 
-std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
+int SparsePoseGraph::num_submaps(const int trajectory_id) {
-    const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   if (trajectory_id >= submap_data_.num_trajectories()) {
-    return {transform::Rigid3d::Identity()};
+    return 0;
   }
+  return submap_data_.num_indices(trajectory_id);
+}
 
-  // Submaps for which we have optimized poses.
+transform::Rigid3d SparsePoseGraph::GetSubmapTransform(
-  std::vector<transform::Rigid3d> result;
+    const mapping::SubmapId& submap_id) {
-  for (int submap_index = 0;
+  common::MutexLocker locker(&mutex_);
-       submap_index != submap_data_.num_indices(trajectory_id);
+  // We already have an optimized pose.
-       ++submap_index) {
+  if (submap_id.trajectory_id <
-    const mapping::SubmapId submap_id{trajectory_id, submap_index};
+          static_cast<int>(optimized_submap_transforms_.size()) &&
-    const auto& submap_data = submap_data_.at(submap_id);
+      submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
-    if (static_cast<size_t>(trajectory_id) <
+                                                    .at(submap_id.trajectory_id)
-            optimized_submap_transforms_.size() &&
+                                                    .size())) {
-        result.size() < optimized_submap_transforms_.at(trajectory_id).size()) {
+    return optimized_submap_transforms_.at(submap_id.trajectory_id)
-      // Submaps for which we have optimized poses.
+        .at(submap_id.submap_index)
-      result.push_back(optimized_submap_transforms_.at(trajectory_id)
+        .pose;
-                           .at(result.size())
-                           .pose);
-    } else {
-      // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submap
-      // is okay, since the member is const.
-      result.push_back(ComputeLocalToGlobalTransform(
-                           optimized_submap_transforms_, trajectory_id) *
-                       submap_data.submap->local_pose());
-    }
   }
-
+  // We have to extrapolate.
-  if (result.empty()) {
+  return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
-    result.push_back(transform::Rigid3d::Identity());
+                                       submap_id.trajectory_id) *
-  }
+         submap_data_.at(submap_id).submap->local_pose();
-  return result;
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(

cartographer/mapping_3d/sparse_pose_graph.h

@@ -81,7 +81,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   }
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
-  std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
+  int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
+  transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id)
       EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;