Change GetAllSubmapData() to MapById. (#597)

cartographer/mapping/id.h

@@ -243,7 +243,9 @@ class MapById {
 
   // Returns an iterator to the element at 'id' or the end iterator if it does
   // not exist.
-  ConstIterator find(const IdType& id) { return ConstIterator(*this, id); }
+  ConstIterator find(const IdType& id) const {
+    return ConstIterator(*this, id);
+  }
 
   // Inserts data (which must not exist already) into a trajectory.
   void Insert(const IdType& id, const DataType& data) {

cartographer/mapping/map_builder.cc

@@ -140,25 +140,17 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
   writer->WriteProto(sparse_pose_graph_->ToProto());
   // Next we serialize all submap data.
   {
-    const auto submap_data = sparse_pose_graph_->GetAllSubmapData();
+    for (const auto& submap_id_data : sparse_pose_graph_->GetAllSubmapData()) {
-    for (int trajectory_id = 0;
-         trajectory_id != static_cast<int>(submap_data.size());
-         ++trajectory_id) {
-      for (int submap_index = 0;
-           submap_index != static_cast<int>(submap_data[trajectory_id].size());
-           ++submap_index) {
       proto::SerializedData proto;
       auto* const submap_proto = proto.mutable_submap();
-        // TODO(whess): Handle trimmed data.
+      submap_proto->mutable_submap_id()->set_trajectory_id(
-        submap_proto->mutable_submap_id()->set_trajectory_id(trajectory_id);
+          submap_id_data.id.trajectory_id);
-        submap_proto->mutable_submap_id()->set_submap_index(submap_index);
+      submap_proto->mutable_submap_id()->set_submap_index(
-        submap_data[trajectory_id][submap_index].submap->ToProto(submap_proto);
+          submap_id_data.id.submap_index);
-        // TODO(whess): Only enable optionally? Resulting pbstream files will be
+      submap_id_data.data.submap->ToProto(submap_proto);
-        // a lot larger now.
       writer->WriteProto(proto);
     }
   }
-  }
   // Next we serialize all node data.
   {
     const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes();
@@ -204,6 +196,16 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
   FinishTrajectory(map_trajectory_id);
   sparse_pose_graph_->FreezeTrajectory(map_trajectory_id);
 
+  MapById<SubmapId, transform::Rigid3d> submap_poses;
+  for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) {
+    for (const proto::Trajectory::Submap& submap_proto :
+         trajectory_proto.submap()) {
+      submap_poses.Insert(SubmapId{trajectory_proto.trajectory_id(),
+                                   submap_proto.submap_index()},
+                          transform::ToRigid3(submap_proto.pose()));
+    }
+  }
+
   for (;;) {
     proto::SerializedData proto;
     if (!reader->ReadProto(&proto)) {
@@ -219,10 +221,9 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
                                            proto.node());
     }
     if (proto.has_submap()) {
-      const transform::Rigid3d submap_pose = transform::ToRigid3(
+      const transform::Rigid3d submap_pose =
-          pose_graph.trajectory(proto.submap().submap_id().trajectory_id())
+          submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(),
-              .submap(proto.submap().submap_id().submap_index())
+                                   proto.submap().submap_id().submap_index()});
-              .pose());
       sparse_pose_graph_->AddSubmapFromProto(map_trajectory_id, submap_pose,
                                              proto.submap());
     }

cartographer/mapping/proto/sparse_pose_graph.proto

@@ -31,17 +31,17 @@ message NodeId {
 
 message SparsePoseGraph {
   message Constraint {
-    // Differentiates between intra-submap (where the scan was inserted into the
+    // Differentiates between intra-submap (where the range data was inserted
-    // submap) and inter-submap constraints (where the scan was not inserted
+    // into the submap) and inter-submap constraints (where the range data was
-    // into the submap).
+    // not inserted into the submap).
     enum Tag {
       INTRA_SUBMAP = 0;
       INTER_SUBMAP = 1;
     }
 
     optional SubmapId submap_id = 1;  // Submap ID.
-    optional NodeId node_id = 2;  // Scan ID.
+    optional NodeId node_id = 2;  // Node ID.
-    // Pose of the scan relative to submap, i.e. taking data from the scan frame
+    // Pose of the node relative to submap, i.e. taking data from the node frame
     // into the submap frame.
     optional transform.proto.Rigid3d relative_pose = 3;
     // Weight of the translational part of the constraint.

cartographer/mapping/proto/trajectory.proto

@@ -24,14 +24,22 @@ message Trajectory {
   // NEXT_ID: 7
   message Node {
     optional int64 timestamp = 1;
-    // Transform from tracking to map frame.
+
+    // Transform from tracking to global map frame.
     optional transform.proto.Rigid3d pose = 5;
   }
 
   message Submap {
+    // Index of this submap within its trajectory.
+    optional int32 submap_index = 2;
+
+    // Transform from submap to global map frame.
     optional transform.proto.Rigid3d pose = 1;
   }
 
+  // ID of this trajectory.
+  optional int32 trajectory_id = 3;
+
   // Time-ordered sequence of Nodes.
   repeated Node node = 1;
 

cartographer/mapping/sparse_pose_graph.cc

@@ -66,14 +66,23 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
 proto::SparsePoseGraph SparsePoseGraph::ToProto() {
   proto::SparsePoseGraph proto;
 
+  std::map<int, proto::Trajectory* const> trajectory_protos;
+  const auto trajectory = [&proto, &trajectory_protos](
+                              const int trajectory_id) -> proto::Trajectory* {
+    if (trajectory_protos.count(trajectory_id) == 0) {
+      auto* const trajectory_proto = proto.add_trajectory();
+      trajectory_proto->set_trajectory_id(trajectory_id);
+      CHECK(trajectory_protos.emplace(trajectory_id, trajectory_proto).second);
+    }
+    return trajectory_protos.at(trajectory_id);
+  };
+
   std::map<NodeId, NodeId> node_id_remapping;  // Due to trimming.
-  std::map<SubmapId, SubmapId> submap_id_remapping;  // Due to trimming.
 
   const auto all_trajectory_nodes = GetTrajectoryNodes();
-  const auto all_submap_data = GetAllSubmapData();
   for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
        ++trajectory_id) {
-    auto* trajectory_proto = proto.add_trajectory();
+    auto* const trajectory_proto = trajectory(trajectory_id);
 
     const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
     for (size_t old_node_index = 0;
@@ -90,21 +99,15 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
         *node_proto->mutable_pose() = transform::ToProto(node.global_pose);
       }
     }
+  }
 
-    const auto& single_trajectory_submap_data = all_submap_data[trajectory_id];
+  for (const auto& submap_id_data : GetAllSubmapData()) {
-    for (size_t old_submap_index = 0;
+    CHECK(submap_id_data.data.submap != nullptr);
-         old_submap_index != single_trajectory_submap_data.size();
+    auto* const submap_proto =
-         ++old_submap_index) {
+        trajectory(submap_id_data.id.trajectory_id)->add_submap();
-      const auto& submap_data = single_trajectory_submap_data[old_submap_index];
+    submap_proto->set_submap_index(submap_id_data.id.submap_index);
-      if (submap_data.submap != nullptr) {
+    *submap_proto->mutable_pose() =
-        submap_id_remapping[SubmapId{static_cast<int>(trajectory_id),
+        transform::ToProto(submap_id_data.data.pose);
-                                     static_cast<int>(old_submap_index)}] =
-            SubmapId{static_cast<int>(trajectory_id),
-                     static_cast<int>(trajectory_proto->submap_size())};
-        *trajectory_proto->add_submap()->mutable_pose() =
-            transform::ToProto(submap_data.pose);
-      }
-    }
   }
 
   for (const auto& constraint : constraints()) {
@@ -115,11 +118,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
         constraint.pose.translation_weight);
     constraint_proto->set_rotation_weight(constraint.pose.rotation_weight);
 
-    const SubmapId submap_id = submap_id_remapping.at(constraint.submap_id);
     constraint_proto->mutable_submap_id()->set_trajectory_id(
-        submap_id.trajectory_id);
+        constraint.submap_id.trajectory_id);
     constraint_proto->mutable_submap_id()->set_submap_index(
-        submap_id.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(

cartographer/mapping/sparse_pose_graph.h

@@ -104,8 +104,9 @@ class SparsePoseGraph {
   // not exist (anymore).
   virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
 
-  // Returns data for all Submaps by trajectory.
+  // Returns data for all submaps.
-  virtual std::vector<std::vector<SubmapData>> GetAllSubmapData() = 0;
+  virtual mapping::MapById<mapping::SubmapId, SubmapData>
+  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

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -539,18 +539,16 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
   return GetSubmapDataUnderLock(submap_id);
 }
 
-std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
 SparsePoseGraph::GetAllSubmapData() {
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>> result;
+  mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
-  for (const int trajectory_id : submap_data_.trajectory_ids()) {
+      submaps;
-    result.resize(trajectory_id + 1);
+  for (const auto& submap_id_data : submap_data_) {
-    for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) {
+    submaps.Insert(submap_id_data.id,
-      result[trajectory_id].resize(submap_id_data.id.submap_index + 1);
+                   GetSubmapDataUnderLock(submap_id_data.id));
-      result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id);
   }
-  }
+  return submaps;
-  return result;
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(

cartographer/mapping_2d/sparse_pose_graph.h

@@ -94,7 +94,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
   mapping::SparsePoseGraph::SubmapData GetSubmapData(
       const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
-  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+  mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
   GetAllSubmapData() EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -568,18 +568,16 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
   return GetSubmapDataUnderLock(submap_id);
 }
 
-std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
 SparsePoseGraph::GetAllSubmapData() {
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>> result;
+  mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
-  for (const int trajectory_id : submap_data_.trajectory_ids()) {
+      submaps;
-    result.resize(trajectory_id + 1);
+  for (const auto& submap_id_data : submap_data_) {
-    for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) {
+    submaps.Insert(submap_id_data.id,
-      result[trajectory_id].resize(submap_id_data.id.submap_index + 1);
+                   GetSubmapDataUnderLock(submap_id_data.id));
-      result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id);
   }
-  }
+  return submaps;
-  return result;
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(

cartographer/mapping_3d/sparse_pose_graph.h

@@ -94,7 +94,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
   mapping::SparsePoseGraph::SubmapData GetSubmapData(
       const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
-  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+  mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
   GetAllSubmapData() EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;