Sparse pose graph no longer uses Submaps* for trajectory identification. (#290)

Related to #256.

PAIR=wohe

cartographer/mapping/map_builder.cc

@@ -78,7 +78,7 @@ int MapBuilder::AddTrajectoryBuilder(
             &sensor_collator_, trajectory_id, expected_sensor_ids,
             common::make_unique<mapping_3d::GlobalTrajectoryBuilder>(
                 trajectory_options.trajectory_builder_3d_options(),
-                sparse_pose_graph_3d_.get())));
+                trajectory_id, sparse_pose_graph_3d_.get())));
   } else {
     CHECK(trajectory_options.has_trajectory_builder_2d_options());
     trajectory_builders_.push_back(
@@ -86,10 +86,8 @@ int MapBuilder::AddTrajectoryBuilder(
             &sensor_collator_, trajectory_id, expected_sensor_ids,
             common::make_unique<mapping_2d::GlobalTrajectoryBuilder>(
                 trajectory_options.trajectory_builder_2d_options(),
-                sparse_pose_graph_2d_.get())));
+                trajectory_id, sparse_pose_graph_2d_.get())));
   }
-  trajectory_ids_.emplace(trajectory_builders_.back()->submaps(),
-                          trajectory_id);
   return trajectory_id;
 }
 
@@ -106,12 +104,6 @@ int MapBuilder::GetBlockingTrajectoryId() const {
   return sensor_collator_.GetBlockingTrajectoryId();
 }
 
-int MapBuilder::GetTrajectoryId(const Submaps* const trajectory) const {
-  const auto trajectory_id = trajectory_ids_.find(trajectory);
-  CHECK(trajectory_id != trajectory_ids_.end());
-  return trajectory_id->second;
-}
-
 proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() {
   return ToProto(sparse_pose_graph_->GetConnectedTrajectories());
 }
@@ -125,10 +117,8 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
            " trajectories.";
   }
 
-  const Submaps* const submaps =
-      trajectory_builders_.at(trajectory_id)->submaps();
   const std::vector<transform::Rigid3d> submap_transforms =
-      sparse_pose_graph_->GetSubmapTransforms(submaps);
+      sparse_pose_graph_->GetSubmapTransforms(trajectory_id);
   if (submap_index < 0 ||
       static_cast<size_t>(submap_index) >= submap_transforms.size()) {
     return "Requested submap " + std::to_string(submap_index) +
@@ -137,6 +127,8 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
            " submaps in this trajectory.";
   }
 
+  const Submaps* const submaps =
+      trajectory_builders_.at(trajectory_id)->submaps();
   response->set_submap_version(submaps->Get(submap_index)->num_range_data);
   submaps->SubmapToProto(submap_index, submap_transforms[submap_index],
                          response);

cartographer/mapping/map_builder.h

@@ -71,9 +71,6 @@ class MapBuilder {
   // unblocked.
   int GetBlockingTrajectoryId() const;
 
-  // Returns the trajectory ID for 'trajectory'.
-  int GetTrajectoryId(const mapping::Submaps* trajectory) const;
-
   // Returns the trajectory connectivity.
   proto::TrajectoryConnectivity GetTrajectoryConnectivity();
 
@@ -97,7 +94,6 @@ class MapBuilder {
 
   sensor::Collator sensor_collator_;
   std::vector<std::unique_ptr<mapping::TrajectoryBuilder>> trajectory_builders_;
-  std::unordered_map<const mapping::Submaps*, int> trajectory_ids_;
 };
 
 }  // namespace mapping

cartographer/mapping/sparse_pose_graph.h

@@ -26,7 +26,6 @@
 #include "cartographer/mapping/id.h"
 #include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
 #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
-#include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_node.h"
 #include "cartographer/transform/rigid_transform.h"
 
@@ -71,11 +70,6 @@ class SparsePoseGraph {
   // Get the current trajectory clusters.
   virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
 
-  // Returns the current optimized transforms for the given 'trajectory'.
-  // TODO(hrapp): Remove this version.
-  virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
-      const Submaps* trajectory) = 0;
-
   // Returns the current optimized transforms for the given 'trajectory_id'.
   virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
       int trajectory_id) = 0;
@@ -83,8 +77,7 @@ class SparsePoseGraph {
   // Returns the transform converting data in the local map frame (i.e. the
   // continuous, non-loop-closed frame) into the global map frame (i.e. the
   // discontinuous, loop-closed frame).
-  virtual transform::Rigid3d GetLocalToGlobalTransform(
+  virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0;
-      const Submaps* submaps) = 0;
 
   // Returns the current optimized trajectories.
   virtual std::vector<std::vector<TrajectoryNode>> GetTrajectoryNodes() = 0;

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -21,8 +21,8 @@ namespace mapping_2d {
 
 GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
     const proto::LocalTrajectoryBuilderOptions& options,
-    SparsePoseGraph* sparse_pose_graph)
+    const int trajectory_id, SparsePoseGraph* sparse_pose_graph)
-    : options_(options),
+    : trajectory_id_(trajectory_id),
       sparse_pose_graph_(sparse_pose_graph),
       local_trajectory_builder_(options) {}
 
@@ -44,7 +44,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
         insertion_result->range_data_in_tracking_2d,
         insertion_result->pose_estimate_2d,
         kalman_filter::Project2D(insertion_result->covariance_estimate),
-        insertion_result->submaps, insertion_result->matching_submap,
+        trajectory_id_, insertion_result->matching_submap,
         insertion_result->insertion_submaps);
   }
 }
@@ -54,8 +54,8 @@ void GlobalTrajectoryBuilder::AddImuData(
     const Eigen::Vector3d& angular_velocity) {
   local_trajectory_builder_.AddImuData(time, linear_acceleration,
                                        angular_velocity);
-  sparse_pose_graph_->AddImuData(local_trajectory_builder_.submaps(), time,
+  sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration,
-                                 linear_acceleration, angular_velocity);
+                                 angular_velocity);
 }
 
 void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

cartographer/mapping_2d/global_trajectory_builder.h

@@ -28,6 +28,7 @@ class GlobalTrajectoryBuilder
     : public mapping::GlobalTrajectoryBuilderInterface {
  public:
   GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
+                          int trajectory_id,
                           SparsePoseGraph* sparse_pose_graph);
   ~GlobalTrajectoryBuilder() override;
 
@@ -48,7 +49,7 @@ class GlobalTrajectoryBuilder
                        const transform::Rigid3d& pose) override;
 
  private:
-  const proto::LocalTrajectoryBuilderOptions options_;
+  const int trajectory_id_;
   SparsePoseGraph* const sparse_pose_graph_;
   LocalTrajectoryBuilder local_trajectory_builder_;
 };

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -236,9 +236,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
                          transform::Embed3D(pose_estimate_2d.cast<float>())));
 
   return common::make_unique<InsertionResult>(InsertionResult{
-      time, &submaps_, matching_submap, insertion_submaps,
+      time, matching_submap, insertion_submaps, tracking_to_tracking_2d,
-      tracking_to_tracking_2d, tracking_2d_to_map, range_data_in_tracking_2d,
+      range_data_in_tracking_2d, pose_estimate_2d, covariance_observation});
-      pose_estimate_2d, covariance_observation});
 }
 
 const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&

cartographer/mapping_2d/local_trajectory_builder.h

@@ -45,11 +45,9 @@ class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
     common::Time time;
-    const mapping::Submaps* submaps;
     const mapping::Submap* matching_submap;
     std::vector<const mapping::Submap*> insertion_submaps;
     transform::Rigid3d tracking_to_tracking_2d;
-    transform::Rigid3d tracking_2d_to_map;
     sensor::RangeData range_data_in_tracking_2d;
     transform::Rigid2d pose_estimate_2d;
     kalman_filter::PoseCovariance covariance_estimate;

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -90,16 +90,13 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 void SparsePoseGraph::AddScan(
     common::Time time, const transform::Rigid3d& tracking_to_pose,
     const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
-    const kalman_filter::Pose2DCovariance& covariance,
+    const kalman_filter::Pose2DCovariance& covariance, const int trajectory_id,
-    const mapping::Submaps* const trajectory,
     const mapping::Submap* const matching_submap,
     const std::vector<const mapping::Submap*>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
-      GetLocalToGlobalTransform(trajectory) * transform::Embed3D(pose));
+      GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
 
   common::MutexLocker locker(&mutex_);
-  trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
-  const int trajectory_id = trajectory_ids_.at(trajectory);
   const int flat_scan_index = trajectory_nodes_.size();
   CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
 
@@ -150,15 +147,13 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
   }
 }
 
-void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
+void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
-                                 common::Time time,
                                  const Eigen::Vector3d& linear_acceleration,
                                  const Eigen::Vector3d& angular_velocity) {
   common::MutexLocker locker(&mutex_);
-  trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
   AddWorkItem([=]() REQUIRES(mutex_) {
-    optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time,
+    optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration,
-                                     linear_acceleration, angular_velocity);
+                                     angular_velocity);
   });
 }
 
@@ -173,7 +168,6 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
                                                .at(node_id.trajectory_id)
                                                .at(node_id.node_index)
                                                .point_cloud_pose;
-
   const int scan_trajectory_id =
       trajectory_nodes_[scan_index].constant_data->trajectory_id;
 
@@ -423,9 +417,10 @@ void SparsePoseGraph::RunOptimization() {
 std::vector<std::vector<mapping::TrajectoryNode>>
 SparsePoseGraph::GetTrajectoryNodes() {
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::TrajectoryNode>> result(
+  std::vector<std::vector<mapping::TrajectoryNode>> result;
-      trajectory_ids_.size());
   for (const auto& node : trajectory_nodes_) {
+    result.resize(
+        std::max<size_t>(result.size(), node.constant_data->trajectory_id + 1));
     result.at(node.constant_data->trajectory_id).push_back(node);
   }
   return result;
@@ -437,11 +432,18 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
 }
 
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
-    const mapping::Submaps* const submaps) {
+    const int trajectory_id) {
-  const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);
+  common::MutexLocker locker(&mutex_);
+  if (submap_states_.size() <= static_cast<size_t>(trajectory_id) ||
+      submap_states_.at(trajectory_id).empty()) {
+    return transform::Rigid3d::Identity();
+  }
+  const auto extrapolated_submap_transforms =
+      ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
   return extrapolated_submap_transforms.back() *
-         submaps->Get(extrapolated_submap_transforms.size() - 1)
+         submap_states_.at(trajectory_id)
-             ->local_pose()
+             .at(extrapolated_submap_transforms.size() - 1)
+             .submap->local_pose()
              .inverse();
 }
 
@@ -450,16 +452,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
   return connected_components_;
 }
 
-std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
-    const mapping::Submaps* const trajectory) {
-  common::MutexLocker locker(&mutex_);
-  if (trajectory_ids_.count(trajectory) == 0) {
-    return {transform::Rigid3d::Identity()};
-  }
-  return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
-                                     trajectory_ids_.at(trajectory));
-}
-
 std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
     const int trajectory_id) {
   common::MutexLocker locker(&mutex_);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -70,23 +70,20 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                const sensor::RangeData& range_data_in_pose,
                const transform::Rigid2d& pose,
                const kalman_filter::Pose2DCovariance& pose_covariance,
-               const mapping::Submaps* trajectory,
+               int trajectory_id, const mapping::Submap* matching_submap,
-               const mapping::Submap* matching_submap,
                const std::vector<const mapping::Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
 
   // Adds new IMU data to be used in the optimization.
-  void AddImuData(const mapping::Submaps* trajectory, common::Time time,
+  void AddImuData(int trajectory_id, common::Time time,
                   const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity);
 
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
-  std::vector<transform::Rigid3d> GetSubmapTransforms(
-      const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
   std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
       EXCLUDES(mutex_) override;
-  transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
+  transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;
   std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
       override EXCLUDES(mutex_);
@@ -209,10 +206,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Current submap transforms used for displaying data.
   std::vector<std::vector<sparse_pose_graph::SubmapData>>
       optimized_submap_transforms_ GUARDED_BY(mutex_);
-
-  // Map from submap pointers to trajectory IDs.
-  std::unordered_map<const mapping::Submaps*, int> trajectory_ids_
-      GUARDED_BY(mutex_);
 };
 
 }  // namespace mapping_2d

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -158,11 +158,12 @@ class SparsePoseGraphTest : public ::testing::Test {
     const sensor::RangeData range_data{
         Eigen::Vector3f::Zero(), new_point_cloud, {}};
     const transform::Rigid2d pose_estimate = noise * current_pose_;
+    constexpr int kTrajectoryId = 0;
     submaps_->InsertRangeData(TransformRangeData(
         range_data, transform::Embed3D(pose_estimate.cast<float>())));
     sparse_pose_graph_->AddScan(common::FromUniversal(0),
                                 transform::Rigid3d::Identity(), range_data,
-                                pose_estimate, covariance, submaps_.get(),
+                                pose_estimate, covariance, kTrajectoryId,
                                 matching_submap, insertion_submaps);
   }
 

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -23,8 +23,9 @@ namespace mapping_3d {
 
 GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
     const proto::LocalTrajectoryBuilderOptions& options,
-    SparsePoseGraph* sparse_pose_graph)
+    const int trajectory_id, SparsePoseGraph* sparse_pose_graph)
-    : sparse_pose_graph_(sparse_pose_graph),
+    : trajectory_id_(trajectory_id),
+      sparse_pose_graph_(sparse_pose_graph),
       local_trajectory_builder_(CreateLocalTrajectoryBuilder(options)) {}
 
 GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
@@ -38,8 +39,8 @@ void GlobalTrajectoryBuilder::AddImuData(
     const Eigen::Vector3d& angular_velocity) {
   local_trajectory_builder_->AddImuData(time, linear_acceleration,
                                         angular_velocity);
-  sparse_pose_graph_->AddImuData(local_trajectory_builder_->submaps(), time,
+  sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration,
-                                 linear_acceleration, angular_velocity);
+                                 angular_velocity);
 }
 
 void GlobalTrajectoryBuilder::AddRangefinderData(
@@ -55,7 +56,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
   sparse_pose_graph_->AddScan(
       insertion_result->time, insertion_result->range_data_in_tracking,
       insertion_result->pose_observation, insertion_result->covariance_estimate,
-      insertion_result->submaps, insertion_result->matching_submap,
+      trajectory_id_, insertion_result->matching_submap,
       insertion_result->insertion_submaps);
 }
 

cartographer/mapping_3d/global_trajectory_builder.h

@@ -31,6 +31,7 @@ class GlobalTrajectoryBuilder
     : public mapping::GlobalTrajectoryBuilderInterface {
  public:
   GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
+                          int trajectory_id,
                           mapping_3d::SparsePoseGraph* sparse_pose_graph);
   ~GlobalTrajectoryBuilder() override;
 
@@ -47,6 +48,7 @@ class GlobalTrajectoryBuilder
   const PoseEstimate& pose_estimate() const override;
 
  private:
+  const int trajectory_id_;
   mapping_3d::SparsePoseGraph* const sparse_pose_graph_;
   std::unique_ptr<LocalTrajectoryBuilderInterface> local_trajectory_builder_;
 };

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -230,7 +230,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
       trajectory_node_index);
   return std::unique_ptr<InsertionResult>(new InsertionResult{
       time, range_data_in_tracking, pose_observation, covariance_estimate,
-      submaps_.get(), matching_submap, insertion_submaps});
+      matching_submap, insertion_submaps});
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/local_trajectory_builder_interface.h

@@ -38,7 +38,6 @@ class LocalTrajectoryBuilderInterface {
     sensor::RangeData range_data_in_tracking;
     transform::Rigid3d pose_observation;
     kalman_filter::PoseCovariance covariance_estimate;
-    const Submaps* submaps;
     const Submap* matching_submap;
     std::vector<const Submap*> insertion_submaps;
   };

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

@@ -406,9 +406,9 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
   const kalman_filter::PoseCovariance kCovariance =
       1e-7 * kalman_filter::PoseCovariance::Identity();
 
-  return std::unique_ptr<InsertionResult>(new InsertionResult{
+  return std::unique_ptr<InsertionResult>(
-      time, range_data_in_tracking, pose_observation, kCovariance,
+      new InsertionResult{time, range_data_in_tracking, pose_observation,
-      submaps_.get(), matching_submap, insertion_submaps});
+                          kCovariance, matching_submap, insertion_submaps});
 }
 
 OptimizingLocalTrajectoryBuilder::State

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -89,15 +89,13 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 void SparsePoseGraph::AddScan(
     common::Time time, const sensor::RangeData& range_data_in_tracking,
     const transform::Rigid3d& pose,
-    const kalman_filter::PoseCovariance& covariance,
+    const kalman_filter::PoseCovariance& covariance, const int trajectory_id,
-    const Submaps* const trajectory, const Submap* const matching_submap,
+    const Submap* const matching_submap,
     const std::vector<const Submap*>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
-      GetLocalToGlobalTransform(trajectory) * pose);
+      GetLocalToGlobalTransform(trajectory_id) * pose);
 
   common::MutexLocker locker(&mutex_);
-  trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
-  const int trajectory_id = trajectory_ids_.at(trajectory);
   const int flat_scan_index = trajectory_nodes_.size();
   CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
 
@@ -150,15 +148,13 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
   }
 }
 
-void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
+void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
-                                 common::Time time,
                                  const Eigen::Vector3d& linear_acceleration,
                                  const Eigen::Vector3d& angular_velocity) {
   common::MutexLocker locker(&mutex_);
-  trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
   AddWorkItem([=]() REQUIRES(mutex_) {
-    optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time,
+    optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration,
-                                     linear_acceleration, angular_velocity);
+                                     angular_velocity);
   });
 }
 
@@ -413,9 +409,10 @@ void SparsePoseGraph::RunOptimization() {
 std::vector<std::vector<mapping::TrajectoryNode>>
 SparsePoseGraph::GetTrajectoryNodes() {
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::TrajectoryNode>> result(
+  std::vector<std::vector<mapping::TrajectoryNode>> result;
-      trajectory_ids_.size());
   for (const auto& node : trajectory_nodes_) {
+    result.resize(
+        std::max<size_t>(result.size(), node.constant_data->trajectory_id + 1));
     result.at(node.constant_data->trajectory_id).push_back(node);
   }
   return result;
@@ -427,11 +424,18 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
 }
 
 transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
-    const mapping::Submaps* const submaps) {
+    const int trajectory_id) {
-  const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);
+  common::MutexLocker locker(&mutex_);
+  if (submap_states_.size() <= static_cast<size_t>(trajectory_id) ||
+      submap_states_.at(trajectory_id).empty()) {
+    return transform::Rigid3d::Identity();
+  }
+  const auto extrapolated_submap_transforms =
+      ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
   return extrapolated_submap_transforms.back() *
-         submaps->Get(extrapolated_submap_transforms.size() - 1)
+         submap_states_.at(trajectory_id)
-             ->local_pose()
+             .at(extrapolated_submap_transforms.size() - 1)
+             .submap->local_pose()
              .inverse();
 }
 
@@ -440,16 +444,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
   return connected_components_;
 }
 
-std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
-    const mapping::Submaps* const trajectory) {
-  common::MutexLocker locker(&mutex_);
-  if (trajectory_ids_.count(trajectory) == 0) {
-    return {transform::Rigid3d::Identity()};
-  }
-  return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
-                                     trajectory_ids_.at(trajectory));
-}
-
 std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
     const int trajectory_id) {
   common::MutexLocker locker(&mutex_);

cartographer/mapping_3d/sparse_pose_graph.h

@@ -69,7 +69,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                const sensor::RangeData& range_data_in_tracking,
                const transform::Rigid3d& pose,
                const kalman_filter::PoseCovariance& pose_covariance,
-               const Submaps* trajectory, const Submap* matching_submap,
+               int trajectory_id, const Submap* matching_submap,
                const std::vector<const Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
 
@@ -78,17 +78,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_);
 
   // Adds new IMU data to be used in the optimization.
-  void AddImuData(const mapping::Submaps* trajectory, common::Time time,
+  void AddImuData(int trajectory_id, common::Time time,
                   const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity);
 
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
-  std::vector<transform::Rigid3d> GetSubmapTransforms(
-      const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
   std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
       EXCLUDES(mutex_) override;
-  transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
+  transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;
   std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
       override EXCLUDES(mutex_);
@@ -209,10 +207,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Current submap transforms used for displaying data.
   std::vector<std::vector<sparse_pose_graph::SubmapData>>
       optimized_submap_transforms_ GUARDED_BY(mutex_);
-
-  // Map from submap pointers to trajectory IDs.
-  std::unordered_map<const mapping::Submaps*, int> trajectory_ids_
-      GUARDED_BY(mutex_);
 };
 
 }  // namespace mapping_3d