Change GetTrajectoryNodes() to MapById. (#603)

cartographer/mapping/map_builder.cc

@@ -153,25 +153,16 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
   }
   // Next we serialize all node data.
   {
-    const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes();
+    for (const auto& node_id_data : sparse_pose_graph_->GetTrajectoryNodes()) {
-    for (int trajectory_id = 0;
+      proto::SerializedData proto;
-         trajectory_id != static_cast<int>(trajectory_nodes.size());
+      auto* const node_proto = proto.mutable_node();
-         ++trajectory_id) {
+      node_proto->mutable_node_id()->set_trajectory_id(
-      for (int node_index = 0;
+          node_id_data.id.trajectory_id);
-           node_index !=
+      node_proto->mutable_node_id()->set_node_index(
-           static_cast<int>(trajectory_nodes[trajectory_id].size());
+          node_id_data.id.node_index);
-           ++node_index) {
+      *node_proto->mutable_node_data() =
-        proto::SerializedData proto;
+          ToProto(*node_id_data.data.constant_data);
-        auto* const node_proto = proto.mutable_node();
+      writer->WriteProto(proto);
-        // TODO(whess): Handle trimmed data.
-        node_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
-        node_proto->mutable_node_id()->set_node_index(node_index);
-        *node_proto->mutable_node_data() =
-            ToProto(*trajectory_nodes[trajectory_id][node_index].constant_data);
-        // TODO(whess): Only enable optionally? Resulting pbstream files will be
-        // a lot larger now.
-        writer->WriteProto(proto);
-      }
     }
     // TODO(whess): Serialize additional sensor data: IMU, odometry.
   }
@@ -206,18 +197,26 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
     }
   }
 
+  MapById<NodeId, transform::Rigid3d> node_poses;
+  for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) {
+    for (const proto::Trajectory::Node& node_proto :
+         trajectory_proto.node()) {
+      node_poses.Insert(NodeId{trajectory_proto.trajectory_id(),
+                               node_proto.node_index()},
+                        transform::ToRigid3(node_proto.pose()));
+    }
+  }
+
   for (;;) {
     proto::SerializedData proto;
     if (!reader->ReadProto(&proto)) {
       break;
     }
     if (proto.has_node()) {
-      const auto& pose_graph_node =
+      const transform::Rigid3d node_pose =
-          pose_graph.trajectory(proto.node().node_id().trajectory_id())
+          node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
-              .node(proto.node().node_id().node_index());
+                               proto.node().node_id().node_index()});
-      const transform::Rigid3d pose =
+      sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, node_pose,
-          transform::ToRigid3(pose_graph_node.pose());
-      sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, pose,
                                            proto.node());
     }
     if (proto.has_submap()) {

cartographer/mapping/proto/trajectory.proto

@@ -21,8 +21,11 @@ import "cartographer/transform/proto/transform.proto";
 option java_outer_classname = "TrajectoryOuterClass";
 
 message Trajectory {
-  // NEXT_ID: 7
+  // NEXT_ID: 8
   message Node {
+    // Index of this node within its trajectory.
+    optional int32 node_index = 7;
+
     optional int64 timestamp = 1;
 
     // Transform from tracking to global map frame.

cartographer/mapping/sparse_pose_graph.cc

@@ -77,28 +77,15 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
     return trajectory_protos.at(trajectory_id);
   };
 
-  std::map<NodeId, NodeId> node_id_remapping;  // Due to trimming.
+  for (const auto& node_id_data : GetTrajectoryNodes()) {
-
+    CHECK(node_id_data.data.constant_data != nullptr);
-  const auto all_trajectory_nodes = GetTrajectoryNodes();
+    auto* const node_proto =
-  for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
+        trajectory(node_id_data.id.trajectory_id)->add_node();
-       ++trajectory_id) {
+    node_proto->set_node_index(node_id_data.id.node_index);
-    auto* const trajectory_proto = trajectory(trajectory_id);
+    node_proto->set_timestamp(
-
+        common::ToUniversal(node_id_data.data.constant_data->time));
-    const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
+    *node_proto->mutable_pose() =
-    for (size_t old_node_index = 0;
+        transform::ToProto(node_id_data.data.global_pose);
-         old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
-      const auto& node = single_trajectory_nodes[old_node_index];
-      if (node.constant_data != nullptr) {
-        node_id_remapping[NodeId{static_cast<int>(trajectory_id),
-                                 static_cast<int>(old_node_index)}] =
-            NodeId{static_cast<int>(trajectory_id),
-                   static_cast<int>(trajectory_proto->node_size())};
-        auto* node_proto = trajectory_proto->add_node();
-        node_proto->set_timestamp(
-            common::ToUniversal(node.constant_data->time));
-        *node_proto->mutable_pose() = transform::ToProto(node.global_pose);
-      }
-    }
   }
 
   for (const auto& submap_id_data : GetAllSubmapData()) {
@@ -123,10 +110,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
     constraint_proto->mutable_submap_id()->set_submap_index(
         constraint.submap_id.submap_index);
 
-    const NodeId node_id = node_id_remapping.at(constraint.node_id);
     constraint_proto->mutable_node_id()->set_trajectory_id(
-        node_id.trajectory_id);
+        constraint.node_id.trajectory_id);
-    constraint_proto->mutable_node_id()->set_node_index(node_id.node_index);
+    constraint_proto->mutable_node_id()->set_node_index(
+        constraint.node_id.node_index);
 
     constraint_proto->set_tag(mapping::ToProto(constraint.tag));
   }

cartographer/mapping/sparse_pose_graph.h

@@ -105,8 +105,7 @@ class SparsePoseGraph {
   virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
 
   // Returns data for all submaps.
-  virtual mapping::MapById<mapping::SubmapId, SubmapData>
+  virtual MapById<SubmapId, SubmapData> GetAllSubmapData() = 0;
-  GetAllSubmapData() = 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
@@ -114,7 +113,7 @@ class SparsePoseGraph {
   virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0;
 
   // Returns the current optimized trajectories.
-  virtual std::vector<std::vector<TrajectoryNode>> GetTrajectoryNodes() = 0;
+  virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0;
 
   // Serializes the constraints and trajectories.
   proto::SparsePoseGraph ToProto();

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -490,19 +490,10 @@ void SparsePoseGraph::RunOptimization() {
   optimized_submap_transforms_ = submap_data;
 }
 
-std::vector<std::vector<mapping::TrajectoryNode>>
+mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
 SparsePoseGraph::GetTrajectoryNodes() {
-  // TODO(cschuet): Rewrite downstream code and switch to MapById.
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::TrajectoryNode>> nodes;
+  return trajectory_nodes_;
-  for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
-    nodes.resize(trajectory_id + 1);
-    for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
-      nodes.at(trajectory_id).resize(node.id.node_index + 1);
-      nodes.at(trajectory_id).at(node.id.node_index) = node.data;
-    }
-  }
-  return nodes;
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

cartographer/mapping_2d/sparse_pose_graph.h

@@ -98,8 +98,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   GetAllSubmapData() EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;
-  std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
-      override EXCLUDES(mutex_);
+  GetTrajectoryNodes() override EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
 
  private:

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -173,6 +173,11 @@ class SparsePoseGraphTest : public ::testing::Test {
     MoveRelativeWithNoise(movement, transform::Rigid2d::Identity());
   }
 
+  template <typename Range>
+  std::vector<int> ToVectorInt(const Range& range) {
+    return std::vector<int>(range.begin(), range.end());
+  }
+
   sensor::PointCloud point_cloud_;
   std::unique_ptr<ActiveSubmaps> active_submaps_;
   common::ThreadPool thread_pool_;
@@ -183,7 +188,7 @@ class SparsePoseGraphTest : public ::testing::Test {
 TEST_F(SparsePoseGraphTest, EmptyMap) {
   sparse_pose_graph_->RunFinalOptimization();
   const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
-  EXPECT_THAT(nodes.size(), ::testing::Eq(0u));
+  EXPECT_TRUE(nodes.empty());
 }
 
 TEST_F(SparsePoseGraphTest, NoMovement) {
@@ -192,13 +197,14 @@ TEST_F(SparsePoseGraphTest, NoMovement) {
   MoveRelative(transform::Rigid2d::Identity());
   sparse_pose_graph_->RunFinalOptimization();
   const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
-  ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
+  ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
-  EXPECT_THAT(nodes[0].size(), ::testing::Eq(3u));
+              ::testing::ContainerEq(std::vector<int>{0}));
-  EXPECT_THAT(nodes[0][0].global_pose,
+  EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u));
+  EXPECT_THAT(nodes.at(mapping::NodeId{0, 0}).global_pose,
               transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
-  EXPECT_THAT(nodes[0][1].global_pose,
+  EXPECT_THAT(nodes.at(mapping::NodeId{0, 1}).global_pose,
               transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
-  EXPECT_THAT(nodes[0][2].global_pose,
+  EXPECT_THAT(nodes.at(mapping::NodeId{0, 2}).global_pose,
               transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
 }
 
@@ -212,10 +218,13 @@ TEST_F(SparsePoseGraphTest, NoOverlappingScans) {
   }
   sparse_pose_graph_->RunFinalOptimization();
   const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
-  ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
+  ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
+              ::testing::ContainerEq(std::vector<int>{0}));
   for (int i = 0; i != 4; ++i) {
     EXPECT_THAT(poses[i],
-                IsNearly(transform::Project2D(nodes[0][i].global_pose), 1e-2))
+                IsNearly(transform::Project2D(
+                             nodes.at(mapping::NodeId{0, i}).global_pose),
+                         1e-2))
         << i;
   }
 }
@@ -230,10 +239,13 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) {
   }
   sparse_pose_graph_->RunFinalOptimization();
   const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
-  ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
+  ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
+              ::testing::ContainerEq(std::vector<int>{0}));
   for (int i = 0; i != 5; ++i) {
     EXPECT_THAT(poses[i],
-                IsNearly(transform::Project2D(nodes[0][i].global_pose), 1e-2))
+                IsNearly(transform::Project2D(
+                             nodes.at(mapping::NodeId{0, i}).global_pose),
+                         1e-2))
         << i;
   }
 }
@@ -255,13 +267,15 @@ TEST_F(SparsePoseGraphTest, OverlappingScans) {
   }
   sparse_pose_graph_->RunFinalOptimization();
   const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
-  ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
+  ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
+              ::testing::ContainerEq(std::vector<int>{0}));
   transform::Rigid2d true_movement =
       ground_truth.front().inverse() * ground_truth.back();
   transform::Rigid2d movement_before = poses.front().inverse() * poses.back();
   transform::Rigid2d error_before = movement_before.inverse() * true_movement;
   transform::Rigid3d optimized_movement =
-      nodes[0].front().global_pose.inverse() * nodes[0].back().global_pose;
+      nodes.BeginOfTrajectory(0)->data.global_pose.inverse() *
+      std::prev(nodes.EndOfTrajectory(0))->data.global_pose;
   transform::Rigid2d optimized_error =
       transform::Project2D(optimized_movement).inverse() * true_movement;
   EXPECT_THAT(std::abs(optimized_error.normalized_angle()),

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -531,19 +531,10 @@ void SparsePoseGraph::RunOptimization() {
   }
 }
 
-std::vector<std::vector<mapping::TrajectoryNode>>
+mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
 SparsePoseGraph::GetTrajectoryNodes() {
-  // TODO(cschuet): Rewrite downstream code and switch to MapById.
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::TrajectoryNode>> nodes;
+  return trajectory_nodes_;
-  for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
-    nodes.resize(trajectory_id + 1);
-    for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
-      nodes.at(trajectory_id).resize(node.id.node_index + 1);
-      nodes.at(trajectory_id).at(node.id.node_index) = node.data;
-    }
-  }
-  return nodes;
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

cartographer/mapping_3d/sparse_pose_graph.h

@@ -98,8 +98,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   GetAllSubmapData() EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;
-  std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
-      override EXCLUDES(mutex_);
+  GetTrajectoryNodes() override EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
 
  private: