ConstantData now has a trajectory_id. (#289)

Related to #256.

PAIR=wohe

cartographer/mapping/sparse_pose_graph.cc

@@ -93,9 +93,8 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
     }
 
     if (!trajectory_nodes.empty()) {
-      const Submaps* const trajectory =
+      for (const auto& transform : GetSubmapTransforms(
-          trajectory_nodes[0].constant_data->trajectory;
+               trajectory_nodes[0].constant_data->trajectory_id)) {
-      for (const auto& transform : GetSubmapTransforms(trajectory)) {
         *trajectory_proto->add_submap()->mutable_pose() =
             transform::ToProto(transform);
       }

cartographer/mapping/sparse_pose_graph.h

@@ -72,9 +72,14 @@ class SparsePoseGraph {
   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;
+
   // 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).

cartographer/mapping/trajectory_node.h

@@ -41,9 +41,7 @@ struct TrajectoryNode {
     sensor::CompressedRangeData range_data_3d;
 
     // Trajectory this node belongs to.
-    // TODO(macmason): The naming here is confusing because 'trajectory'
+    int trajectory_id;
-    // doesn't seem like a good name for a Submaps*. Sort this out.
-    const Submaps* trajectory;
 
     // Transform from the 3D 'tracking' frame to the 'pose' frame of the range
     // data, which contains roll, pitch and height for 2D. In 3D this is always

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -105,8 +105,8 @@ void SparsePoseGraph::AddScan(
 
   constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
       time, range_data_in_pose,
-      Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), trajectory,
+      Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
-      tracking_to_pose});
+      trajectory_id, tracking_to_pose});
   trajectory_nodes_.push_back(mapping::TrajectoryNode{
       &constant_node_data_.back(), optimized_pose,
   });
@@ -174,9 +174,8 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
                                                .at(node_id.node_index)
                                                .point_cloud_pose;
 
-  const mapping::Submaps* const scan_trajectory =
+  const int scan_trajectory_id =
-      trajectory_nodes_[scan_index].constant_data->trajectory;
+      trajectory_nodes_[scan_index].constant_data->trajectory_id;
-  const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
 
   // Only globally match against submaps not in this trajectory.
   if (scan_trajectory_id != submap_id.trajectory_id &&
@@ -242,8 +241,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   ++num_nodes_in_trajectory_[matching_id.trajectory_id];
   const mapping::TrajectoryNode::ConstantData* const scan_data =
       trajectory_nodes_[scan_index].constant_data;
-  CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
+  CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
-           matching_id.trajectory_id);
   optimization_problem_.AddTrajectoryNode(
       matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
   for (const mapping::Submap* submap : insertion_submaps) {
@@ -396,23 +394,21 @@ void SparsePoseGraph::RunOptimization() {
                                .point_cloud_pose);
   }
   // Extrapolate all point cloud poses that were added later.
-  std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
+  std::unordered_map<int, transform::Rigid3d> extrapolation_transforms;
-      extrapolation_transforms;
   for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
-    const mapping::Submaps* trajectory =
+    const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id;
-        trajectory_nodes_[i].constant_data->trajectory;
+    if (extrapolation_transforms.count(trajectory_id) == 0) {
-    if (extrapolation_transforms.count(trajectory) == 0) {
       const auto new_submap_transforms = ExtrapolateSubmapTransforms(
-          optimization_problem_.submap_data(), trajectory);
+          optimization_problem_.submap_data(), trajectory_id);
-      const auto old_submap_transforms =
+      const auto old_submap_transforms = ExtrapolateSubmapTransforms(
-          ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
+          optimized_submap_transforms_, trajectory_id);
       CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
-      extrapolation_transforms[trajectory] =
+      extrapolation_transforms[trajectory_id] =
           transform::Rigid3d(new_submap_transforms.back() *
                              old_submap_transforms.back().inverse());
     }
     trajectory_nodes_[i].pose =
-        extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
+        extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose;
   }
   optimized_submap_transforms_ = optimization_problem_.submap_data();
   connected_components_ = trajectory_connectivity_.ConnectedComponents();
@@ -430,8 +426,7 @@ SparsePoseGraph::GetTrajectoryNodes() {
   std::vector<std::vector<mapping::TrajectoryNode>> result(
       trajectory_ids_.size());
   for (const auto& node : trajectory_nodes_) {
-    result.at(trajectory_ids_.at(node.constant_data->trajectory))
+    result.at(node.constant_data->trajectory_id).push_back(node);
-        .push_back(node);
   }
   return result;
 }
@@ -458,17 +453,24 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
 std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
     const mapping::Submaps* const trajectory) {
   common::MutexLocker locker(&mutex_);
-  return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
+  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_);
+  return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
+                                     trajectory_id);
 }
 
 std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
         submap_transforms,
-    const mapping::Submaps* const trajectory) const {
+    const size_t trajectory_id) const {
-  if (trajectory_ids_.count(trajectory) == 0) {
-    return {transform::Rigid3d::Identity()};
-  }
-  const size_t trajectory_id = trajectory_ids_.at(trajectory);
   if (trajectory_id >= submap_states_.size()) {
     return {transform::Rigid3d::Identity()};
   }

cartographer/mapping_2d/sparse_pose_graph.h

@@ -84,6 +84,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   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)
       EXCLUDES(mutex_) override;
   std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
@@ -153,7 +155,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
       const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
           submap_transforms,
-      const mapping::Submaps* trajectory) const REQUIRES(mutex_);
+      size_t trajectory_id) const REQUIRES(mutex_);
 
   const mapping::proto::SparsePoseGraphOptions options_;
   common::Mutex mutex_;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -103,7 +103,7 @@ void SparsePoseGraph::AddScan(
 
   constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
       time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
-      sensor::Compress(range_data_in_tracking), trajectory,
+      sensor::Compress(range_data_in_tracking), trajectory_id,
       transform::Rigid3d::Identity()});
   trajectory_nodes_.push_back(
       mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
@@ -173,10 +173,8 @@ 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 =
-  const mapping::Submaps* const scan_trajectory =
+      trajectory_nodes_[scan_index].constant_data->trajectory_id;
-      trajectory_nodes_[scan_index].constant_data->trajectory;
-  const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
 
   // Only globally match against submaps not in this trajectory.
   if (scan_trajectory_id != submap_id.trajectory_id &&
@@ -238,8 +236,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   ++num_nodes_in_trajectory_[matching_id.trajectory_id];
   const mapping::TrajectoryNode::ConstantData* const scan_data =
       trajectory_nodes_[scan_index].constant_data;
-  CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
+  CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
-           matching_id.trajectory_id);
   optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
                                           scan_data->time, optimized_pose);
   for (const Submap* submap : insertion_submaps) {
@@ -387,23 +384,21 @@ void SparsePoseGraph::RunOptimization() {
                                     .point_cloud_pose;
   }
   // Extrapolate all point cloud poses that were added later.
-  std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
+  std::unordered_map<int, transform::Rigid3d> extrapolation_transforms;
-      extrapolation_transforms;
   for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
-    const mapping::Submaps* trajectory =
+    const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id;
-        trajectory_nodes_[i].constant_data->trajectory;
+    if (extrapolation_transforms.count(trajectory_id) == 0) {
-    if (extrapolation_transforms.count(trajectory) == 0) {
       const auto new_submap_transforms = ExtrapolateSubmapTransforms(
-          optimization_problem_.submap_data(), trajectory);
+          optimization_problem_.submap_data(), trajectory_id);
-      const auto old_submap_transforms =
+      const auto old_submap_transforms = ExtrapolateSubmapTransforms(
-          ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
+          optimized_submap_transforms_, trajectory_id);
       CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
-      extrapolation_transforms[trajectory] =
+      extrapolation_transforms[trajectory_id] =
           transform::Rigid3d(new_submap_transforms.back() *
                              old_submap_transforms.back().inverse());
     }
     trajectory_nodes_[i].pose =
-        extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
+        extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose;
   }
   optimized_submap_transforms_ = optimization_problem_.submap_data();
   connected_components_ = trajectory_connectivity_.ConnectedComponents();
@@ -421,8 +416,7 @@ SparsePoseGraph::GetTrajectoryNodes() {
   std::vector<std::vector<mapping::TrajectoryNode>> result(
       trajectory_ids_.size());
   for (const auto& node : trajectory_nodes_) {
-    result.at(trajectory_ids_.at(node.constant_data->trajectory))
+    result.at(node.constant_data->trajectory_id).push_back(node);
-        .push_back(node);
   }
   return result;
 }
@@ -449,17 +443,24 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
 std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
     const mapping::Submaps* const trajectory) {
   common::MutexLocker locker(&mutex_);
-  return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
+  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_);
+  return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
+                                     trajectory_id);
 }
 
 std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
         submap_transforms,
-    const mapping::Submaps* const trajectory) const {
+    const size_t trajectory_id) const {
-  if (trajectory_ids_.count(trajectory) == 0) {
-    return {transform::Rigid3d::Identity()};
-  }
-  const size_t trajectory_id = trajectory_ids_.at(trajectory);
   if (trajectory_id >= submap_states_.size()) {
     return {transform::Rigid3d::Identity()};
   }

cartographer/mapping_3d/sparse_pose_graph.h

@@ -86,6 +86,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   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)
       EXCLUDES(mutex_) override;
   std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
@@ -153,7 +155,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
       const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
           submap_transforms,
-      const mapping::Submaps* trajectory) const REQUIRES(mutex_);
+      size_t trajectory_id) const REQUIRES(mutex_);
 
   const mapping::proto::SparsePoseGraphOptions options_;
   common::Mutex mutex_;