Using new serialization format. (#1174)

Updates everyone to use the new serialization format.

A corresponding PR will be made in cartographer_ros for the various tools.

cartographer/cloud/client/map_builder_stub.cc

@@ -25,6 +25,7 @@
 #include "cartographer/cloud/internal/handlers/write_state_handler.h"
 #include "cartographer/cloud/internal/sensor/serialization.h"
 #include "cartographer/cloud/proto/map_builder_service.pb.h"
+#include "cartographer/io/proto_stream_deserializer.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -111,11 +112,8 @@ void MapBuilderStub::SerializeState(io::ProtoStreamWriterInterface* writer) {
   proto::WriteStateResponse response;
   while (client.StreamRead(&response)) {
     switch (response.state_chunk_case()) {
-      case proto::WriteStateResponse::kPoseGraph:
+      case proto::WriteStateResponse::kHeader:
-        writer->WriteProto(response.pose_graph());
+        writer->WriteProto(response.header());
-        break;
-      case proto::WriteStateResponse::kAllTrajectoryBuilderOptions:
-        writer->WriteProto(response.all_trajectory_builder_options());
         break;
       case proto::WriteStateResponse::kSerializedData:
         writer->WriteProto(response.serialized_data());
@@ -132,21 +130,25 @@ void MapBuilderStub::LoadState(io::ProtoStreamReaderInterface* reader,
     LOG(FATAL) << "Not implemented";
   }
   async_grpc::Client<handlers::LoadStateSignature> client(client_channel_);
+
+  io::ProtoStreamDeserializer deserializer(reader);
   // Request with a PoseGraph proto is sent first.
   {
     proto::LoadStateRequest request;
-    CHECK(reader->ReadProto(request.mutable_pose_graph()));
+    *request.mutable_pose_graph() = deserializer.pose_graph();
     CHECK(client.Write(request));
   }
   // Request with an AllTrajectoryBuilderOptions should be second.
   {
     proto::LoadStateRequest request;
-    CHECK(reader->ReadProto(request.mutable_all_trajectory_builder_options()));
+    *request.mutable_all_trajectory_builder_options() =
+        deserializer.all_trajectory_builder_options();
     CHECK(client.Write(request));
   }
   // Multiple requests with SerializedData are sent after.
   proto::LoadStateRequest request;
-  while (reader->ReadProto(request.mutable_serialized_data())) {
+  while (
+      deserializer.ReadNextSerializedData(request.mutable_serialized_data())) {
     CHECK(client.Write(request));
   }
 

cartographer/cloud/internal/handlers/write_state_handler.cc

@@ -36,11 +36,9 @@ void WriteStateHandler::OnRequest(const google::protobuf::Empty& request) {
         }
 
         auto response = common::make_unique<proto::WriteStateResponse>();
-        if (proto->GetTypeName() == "cartographer.mapping.proto.PoseGraph") {
+        if (proto->GetTypeName() ==
-          response->mutable_pose_graph()->CopyFrom(*proto);
+            "cartographer.mapping.proto.SerializationHeader") {
-        } else if (proto->GetTypeName() ==
+          response->mutable_header()->CopyFrom(*proto);
-                   "cartographer.mapping.proto.AllTrajectoryBuilderOptions") {
-          response->mutable_all_trajectory_builder_options()->CopyFrom(*proto);
         } else if (proto->GetTypeName() ==
                    "cartographer.mapping.proto.SerializedData") {
           response->mutable_serialized_data()->CopyFrom(*proto);

cartographer/cloud/proto/map_builder_service.proto

@@ -185,10 +185,8 @@ message GetConstraintsResponse {
 
 message WriteStateResponse {
   oneof state_chunk {
-    cartographer.mapping.proto.PoseGraph pose_graph = 1;
+    cartographer.mapping.proto.SerializationHeader header = 1;
-    cartographer.mapping.proto.AllTrajectoryBuilderOptions
+    cartographer.mapping.proto.SerializedData serialized_data = 2;
-        all_trajectory_builder_options = 2;
-    cartographer.mapping.proto.SerializedData serialized_data = 3;
   }
 }
 
@@ -280,7 +278,7 @@ service MapBuilderService {
 
   // Checks whether the trajectory is frozen.
   rpc IsTrajectoryFrozen(IsTrajectoryFrozenRequest)
-  	  returns (IsTrajectoryFrozenResponse);
+      returns (IsTrajectoryFrozenResponse);
 
   // Requests a PoseGraph to call RunFinalOptimization.
   rpc RunFinalOptimization(google.protobuf.Empty)

cartographer/ground_truth/autogenerate_ground_truth_main.cc

@@ -21,6 +21,7 @@
 #include "cartographer/common/port.h"
 #include "cartographer/ground_truth/proto/relations.pb.h"
 #include "cartographer/io/proto_stream.h"
+#include "cartographer/io/proto_stream_deserializer.h"
 #include "cartographer/mapping/proto/pose_graph.pb.h"
 #include "cartographer/transform/transform.h"
 #include "gflags/gflags.h"
@@ -170,13 +171,9 @@ void Run(const std::string& pose_graph_filename,
          const double outlier_threshold_meters,
          const double outlier_threshold_radians) {
   LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
-  mapping::proto::PoseGraph pose_graph;
+  mapping::proto::PoseGraph pose_graph =
-  {
+      io::DeserializePoseGraphFromFile(pose_graph_filename);
-    io::ProtoStreamReader reader(pose_graph_filename);
+
-    CHECK(reader.ReadProto(&pose_graph));
-    CHECK_EQ(pose_graph.trajectory_size(), 1)
-        << "Only pose graphs containing a single trajectory are supported.";
-  }
   LOG(INFO) << "Autogenerating ground truth relations...";
   const proto::GroundTruth ground_truth =
       GenerateGroundTruth(pose_graph, min_covered_distance,

cartographer/ground_truth/compute_relations_metrics_main.cc

@@ -28,6 +28,7 @@
 #include "cartographer/ground_truth/proto/relations.pb.h"
 #include "cartographer/ground_truth/relations_text_file.h"
 #include "cartographer/io/proto_stream.h"
+#include "cartographer/io/proto_stream_deserializer.h"
 #include "cartographer/mapping/proto/pose_graph.pb.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -126,7 +127,7 @@ void WriteRelationMetricsToFile(const std::vector<Error>& errors,
          "expected_translation_x,expected_translation_y,expected_"
          "translation_z,expected_rotation_w,expected_rotation_x,"
          "expected_rotation_y,expected_rotation_z,covered_distance\n";
-  for (size_t relation_index = 0; relation_index < ground_truth.relation_size();
+  for (int relation_index = 0; relation_index < ground_truth.relation_size();
        ++relation_index) {
     const Error& error = errors[relation_index];
     const proto::Relation& relation = ground_truth.relation(relation_index);
@@ -172,13 +173,9 @@ void Run(const std::string& pose_graph_filename,
          const bool read_text_file_with_unix_timestamps,
          const bool write_relation_metrics) {
   LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
-  mapping::proto::PoseGraph pose_graph;
+  mapping::proto::PoseGraph pose_graph =
-  {
+      io::DeserializePoseGraphFromFile(pose_graph_filename);
-    io::ProtoStreamReader reader(pose_graph_filename);
+
-    CHECK(reader.ReadProto(&pose_graph));
-    CHECK_EQ(pose_graph.trajectory_size(), 1)
-        << "Only pose graphs containing a single trajectory are supported.";
-  }
   const transform::TransformInterpolationBuffer transform_interpolation_buffer(
       pose_graph.trajectory(0));
 

cartographer/io/proto_stream_deserializer.cc

@@ -17,6 +17,7 @@
 #include "cartographer/io/proto_stream_deserializer.h"
 
 #include "cartographer/io/internal/mapping_state_serialization.h"
+#include "cartographer/io/proto_stream.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -35,6 +36,13 @@ bool IsVersionSupported(const mapping::proto::SerializationHeader& header) {
 
 }  // namespace
 
+mapping::proto::PoseGraph DeserializePoseGraphFromFile(
+    const std::string& file_name) {
+  ProtoStreamReader reader(file_name);
+  ProtoStreamDeserializer deserializer(&reader);
+  return deserializer.pose_graph();
+}
+
 ProtoStreamDeserializer::ProtoStreamDeserializer(
     ProtoStreamReaderInterface* const reader)
     : reader_(reader), header_(ReadHeaderOrDie(reader)) {

cartographer/io/proto_stream_deserializer.h

@@ -25,6 +25,10 @@
 namespace cartographer {
 namespace io {
 
+// Helper function for deserializing the PoseGraph from a proto stream file.
+mapping::proto::PoseGraph DeserializePoseGraphFromFile(
+    const std::string& file_name);
+
 // Helper for deserializing a previously serialized mapping state from a
 // proto stream, abstracting away the format parsing logic.
 class ProtoStreamDeserializer {

cartographer/mapping/map_builder.cc

@@ -18,6 +18,8 @@
 
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/time.h"
+#include "cartographer/io/internal/mapping_state_serialization.h"
+#include "cartographer/io/proto_stream_deserializer.h"
 #include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h"
 #include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"
 #include "cartographer/mapping/internal/2d/pose_graph_2d.h"
@@ -35,6 +37,23 @@
 namespace cartographer {
 namespace mapping {
 
+namespace {
+
+using mapping::proto::SerializedData;
+
+std::vector<std::string> SelectRangeSensorIds(
+    const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
+  std::vector<std::string> range_sensor_ids;
+  for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) {
+    if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) {
+      range_sensor_ids.push_back(sensor_id.id);
+    }
+  }
+  return range_sensor_ids;
+}
+
+}  // namespace
+
 proto::MapBuilderOptions CreateMapBuilderOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::MapBuilderOptions options;
@@ -51,17 +70,6 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
   return options;
 }
 
-std::vector<std::string> SelectRangeSensorIds(
-    const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
-  std::vector<std::string> range_sensor_ids;
-  for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) {
-    if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) {
-      range_sensor_ids.push_back(sensor_id.id);
-    }
-  }
-  return range_sensor_ids;
-}
-
 MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
     : options_(options), thread_pool_(options.num_background_threads()) {
   CHECK(options.use_trajectory_builder_2d() ^
@@ -176,8 +184,7 @@ void MapBuilder::FinishTrajectory(const int trajectory_id) {
 }
 
 std::string MapBuilder::SubmapToProto(
-    const mapping::SubmapId& submap_id,
+    const SubmapId& submap_id, proto::SubmapQuery::Response* const response) {
-    proto::SubmapQuery::Response* const response) {
   if (submap_id.trajectory_id < 0 ||
       submap_id.trajectory_id >= num_trajectory_builders()) {
     return "Requested submap from trajectory " +
@@ -196,146 +203,18 @@ std::string MapBuilder::SubmapToProto(
 }
 
 void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
-  // We serialize the pose graph followed by all the data referenced in it.
+  io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer);
-  writer->WriteProto(pose_graph_->ToProto());
-  // Serialize trajectory builder options.
-  {
-    proto::AllTrajectoryBuilderOptions all_builder_options_proto;
-    for (const auto& options_with_sensor_ids :
-         all_trajectory_builder_options_) {
-      *all_builder_options_proto.add_options_with_sensor_ids() =
-          options_with_sensor_ids;
-    }
-    CHECK_EQ(all_trajectory_builder_options_.size(),
-             all_builder_options_proto.options_with_sensor_ids_size());
-    writer->WriteProto(all_builder_options_proto);
-  }
-  // Next we serialize all submap data.
-  {
-    for (const auto& submap_id_data : pose_graph_->GetAllSubmapData()) {
-      proto::LegacySerializedData proto;
-      auto* const submap_proto = proto.mutable_submap();
-      submap_proto->mutable_submap_id()->set_trajectory_id(
-          submap_id_data.id.trajectory_id);
-      submap_proto->mutable_submap_id()->set_submap_index(
-          submap_id_data.id.submap_index);
-      submap_id_data.data.submap->ToProto(
-          submap_proto, true /* include_probability_grid_data */);
-      writer->WriteProto(proto);
-    }
-  }
-  // Next we serialize all node data.
-  {
-    for (const auto& node_id_data : pose_graph_->GetTrajectoryNodes()) {
-      proto::LegacySerializedData proto;
-      auto* const node_proto = proto.mutable_node();
-      node_proto->mutable_node_id()->set_trajectory_id(
-          node_id_data.id.trajectory_id);
-      node_proto->mutable_node_id()->set_node_index(node_id_data.id.node_index);
-      *node_proto->mutable_node_data() =
-          ToProto(*node_id_data.data.constant_data);
-      writer->WriteProto(proto);
-    }
-  }
-  // Next we serialize IMU data from the pose graph.
-  {
-    const auto all_imu_data = pose_graph_->GetImuData();
-    for (const int trajectory_id : all_imu_data.trajectory_ids()) {
-      for (const auto& imu_data : all_imu_data.trajectory(trajectory_id)) {
-        proto::LegacySerializedData proto;
-        auto* const imu_data_proto = proto.mutable_imu_data();
-        imu_data_proto->set_trajectory_id(trajectory_id);
-        *imu_data_proto->mutable_imu_data() = sensor::ToProto(imu_data);
-        writer->WriteProto(proto);
-      }
-    }
-  }
-  // Next we serialize odometry data from the pose graph.
-  {
-    const auto all_odometry_data = pose_graph_->GetOdometryData();
-    for (const int trajectory_id : all_odometry_data.trajectory_ids()) {
-      for (const auto& odometry_data :
-           all_odometry_data.trajectory(trajectory_id)) {
-        proto::LegacySerializedData proto;
-        auto* const odometry_data_proto = proto.mutable_odometry_data();
-        odometry_data_proto->set_trajectory_id(trajectory_id);
-        *odometry_data_proto->mutable_odometry_data() =
-            sensor::ToProto(odometry_data);
-        writer->WriteProto(proto);
-      }
-    }
-  }
-  // Next we serialize all fixed frame pose data from the pose graph.
-  {
-    const auto all_fixed_frame_pose_data = pose_graph_->GetFixedFramePoseData();
-    for (const int trajectory_id : all_fixed_frame_pose_data.trajectory_ids()) {
-      for (const auto& fixed_frame_pose_data :
-           all_fixed_frame_pose_data.trajectory(trajectory_id)) {
-        proto::LegacySerializedData proto;
-        auto* const fixed_frame_pose_data_proto =
-            proto.mutable_fixed_frame_pose_data();
-        fixed_frame_pose_data_proto->set_trajectory_id(trajectory_id);
-        *fixed_frame_pose_data_proto->mutable_fixed_frame_pose_data() =
-            sensor::ToProto(fixed_frame_pose_data);
-        writer->WriteProto(proto);
-      }
-    }
-  }
-  // Next we serialize all trajectory data.
-  {
-    const auto all_trajectory_data = pose_graph_->GetTrajectoryData();
-    for (const auto& trajectory_data : all_trajectory_data) {
-      proto::LegacySerializedData proto;
-      auto* const trajectory_data_proto = proto.mutable_trajectory_data();
-      trajectory_data_proto->set_trajectory_id(trajectory_data.first);
-      trajectory_data_proto->set_gravity_constant(
-          trajectory_data.second.gravity_constant);
-      *trajectory_data_proto->mutable_imu_calibration() = transform::ToProto(
-          Eigen::Quaterniond(trajectory_data.second.imu_calibration[0],
-                             trajectory_data.second.imu_calibration[1],
-                             trajectory_data.second.imu_calibration[2],
-                             trajectory_data.second.imu_calibration[3]));
-      if (trajectory_data.second.fixed_frame_origin_in_map.has_value()) {
-        *trajectory_data_proto->mutable_fixed_frame_origin_in_map() =
-            transform::ToProto(
-                trajectory_data.second.fixed_frame_origin_in_map.value());
-      }
-      writer->WriteProto(proto);
-    }
-  }
-  // Next we serialize all landmark data.
-  {
-    const std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
-        all_landmark_nodes = pose_graph_->GetLandmarkNodes();
-    for (const auto& node : all_landmark_nodes) {
-      for (const auto& observation : node.second.landmark_observations) {
-        proto::LegacySerializedData proto;
-        auto* landmark_data_proto = proto.mutable_landmark_data();
-        landmark_data_proto->set_trajectory_id(observation.trajectory_id);
-        landmark_data_proto->mutable_landmark_data()->set_timestamp(
-            common::ToUniversal(observation.time));
-        auto* observation_proto = landmark_data_proto->mutable_landmark_data()
-                                      ->add_landmark_observations();
-        observation_proto->set_id(node.first);
-        *observation_proto->mutable_landmark_to_tracking_transform() =
-            transform::ToProto(observation.landmark_to_tracking_transform);
-        observation_proto->set_translation_weight(
-            observation.translation_weight);
-        observation_proto->set_rotation_weight(observation.rotation_weight);
-        writer->WriteProto(proto);
-      }
-    }
-  }
 }
 
 void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
                            bool load_frozen_state) {
-  proto::PoseGraph pose_graph_proto;
+  io::ProtoStreamDeserializer deserializer(reader);
-  CHECK(reader->ReadProto(&pose_graph_proto));
+
-  proto::AllTrajectoryBuilderOptions all_builder_options_proto;
+  // Create a copy of the pose_graph_proto, such that we can re-write the
-  CHECK(reader->ReadProto(&all_builder_options_proto));
+  // trajectory ids.
-  CHECK_EQ(pose_graph_proto.trajectory_size(),
+  proto::PoseGraph pose_graph_proto = deserializer.pose_graph();
-           all_builder_options_proto.options_with_sensor_ids_size());
+  const auto& all_builder_options_proto =
+      deserializer.all_trajectory_builder_options();
 
   std::map<int, int> trajectory_remapping;
   for (auto& trajectory_proto : *pose_graph_proto.mutable_trajectory()) {
@@ -389,55 +268,76 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
                                  transform::ToRigid3(landmark.global_pose()));
   }
 
-  for (;;) {
+  SerializedData proto;
-    proto::LegacySerializedData proto;
+  while (deserializer.ReadNextSerializedData(&proto)) {
-    if (!reader->ReadProto(&proto)) {
+    switch (proto.data_case()) {
-      break;
+      case SerializedData::kPoseGraph:
-    }
+        LOG(ERROR) << "Found multiple serialized `PoseGraph`. Serialized "
-    if (proto.has_node()) {
+                      "stream likely corrupt!.";
-      proto.mutable_node()->mutable_node_id()->set_trajectory_id(
+        break;
-          trajectory_remapping.at(proto.node().node_id().trajectory_id()));
+      case SerializedData::kAllTrajectoryBuilderOptions:
-      const transform::Rigid3d node_pose =
+        LOG(ERROR) << "Found multiple serialized "
-          node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
+                      "`AllTrajectoryBuilderOptions`. Serialized stream likely "
-                               proto.node().node_id().node_index()});
+                      "corrupt!.";
-      pose_graph_->AddNodeFromProto(node_pose, proto.node());
+        break;
-    }
+      case SerializedData::kSubmap: {
-    if (proto.has_submap()) {
+        proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
-      proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
+            trajectory_remapping.at(
-          trajectory_remapping.at(proto.submap().submap_id().trajectory_id()));
+                proto.submap().submap_id().trajectory_id()));
-      const transform::Rigid3d submap_pose =
+        const transform::Rigid3d& submap_pose = submap_poses.at(
-          submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(),
+            SubmapId{proto.submap().submap_id().trajectory_id(),
-                                   proto.submap().submap_id().submap_index()});
+                     proto.submap().submap_id().submap_index()});
-      pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
+        pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
-    }
+        break;
-    if (proto.has_trajectory_data()) {
+      }
-      proto.mutable_trajectory_data()->set_trajectory_id(
+      case SerializedData::kNode: {
-          trajectory_remapping.at(proto.trajectory_data().trajectory_id()));
+        proto.mutable_node()->mutable_node_id()->set_trajectory_id(
-      pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data());
+            trajectory_remapping.at(proto.node().node_id().trajectory_id()));
-    }
+        const transform::Rigid3d& node_pose =
-    if (!load_frozen_state) {
+            node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
-      if (proto.has_imu_data()) {
+                                 proto.node().node_id().node_index()});
+        pose_graph_->AddNodeFromProto(node_pose, proto.node());
+        break;
+      }
+      case SerializedData::kTrajectoryData: {
+        proto.mutable_trajectory_data()->set_trajectory_id(
+            trajectory_remapping.at(proto.trajectory_data().trajectory_id()));
+        pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data());
+        break;
+      }
+      case SerializedData::kImuData: {
+        if (load_frozen_state) break;
         pose_graph_->AddImuData(
             trajectory_remapping.at(proto.imu_data().trajectory_id()),
             sensor::FromProto(proto.imu_data().imu_data()));
+        break;
       }
-      if (proto.has_odometry_data()) {
+      case SerializedData::kOdometryData: {
+        if (load_frozen_state) break;
         pose_graph_->AddOdometryData(
             trajectory_remapping.at(proto.odometry_data().trajectory_id()),
             sensor::FromProto(proto.odometry_data().odometry_data()));
+        break;
       }
-      if (proto.has_fixed_frame_pose_data()) {
+      case SerializedData::kFixedFramePoseData: {
+        if (load_frozen_state) break;
         pose_graph_->AddFixedFramePoseData(
             trajectory_remapping.at(
                 proto.fixed_frame_pose_data().trajectory_id()),
             sensor::FromProto(
                 proto.fixed_frame_pose_data().fixed_frame_pose_data()));
+        break;
       }
-      if (proto.has_landmark_data()) {
+      case SerializedData::kLandmarkData: {
+        if (load_frozen_state) break;
         pose_graph_->AddLandmarkData(
             trajectory_remapping.at(proto.landmark_data().trajectory_id()),
             sensor::FromProto(proto.landmark_data().landmark_data()));
+        break;
       }
+      default:
+        LOG(WARNING) << "Skipping unknown message type in stream: "
+                     << proto.GetTypeName();
     }
   }
 
@@ -447,7 +347,7 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
     for (const proto::PoseGraph::Constraint& constraint_proto :
          pose_graph_proto.constraint()) {
       if (constraint_proto.tag() !=
-          mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) {
+          proto::PoseGraph::Constraint::INTRA_SUBMAP) {
         continue;
       }
       pose_graph_->AddNodeToSubmap(