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.
2018-05-30 13:31:33 +02:00 · 2018-05-30 13:31:33 +02:00 · 73d18e5fc5
parent 87370371ec
commit 73d18e5fc5
8 changed files with 118 additions and 214 deletions
--- a/cartographer/cloud/client/map_builder_stub.cc
+++ b/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:
-        writer->WriteProto(response.pose_graph());
-        break;
-      case proto::WriteStateResponse::kAllTrajectoryBuilderOptions:
-        writer->WriteProto(response.all_trajectory_builder_options());
+      case proto::WriteStateResponse::kHeader:
+        writer->WriteProto(response.header());
        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));
  }

--- a/cartographer/cloud/internal/handlers/write_state_handler.cc
+++ b/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") {
-          response->mutable_pose_graph()->CopyFrom(*proto);
-        } else if (proto->GetTypeName() ==
-                   "cartographer.mapping.proto.AllTrajectoryBuilderOptions") {
-          response->mutable_all_trajectory_builder_options()->CopyFrom(*proto);
+        if (proto->GetTypeName() ==
+            "cartographer.mapping.proto.SerializationHeader") {
+          response->mutable_header()->CopyFrom(*proto);
        } else if (proto->GetTypeName() ==
                   "cartographer.mapping.proto.SerializedData") {
          response->mutable_serialized_data()->CopyFrom(*proto);
--- a/cartographer/cloud/proto/map_builder_service.proto
+++ b/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.AllTrajectoryBuilderOptions
-        all_trajectory_builder_options = 2;
-    cartographer.mapping.proto.SerializedData serialized_data = 3;
+    cartographer.mapping.proto.SerializationHeader header = 1;
+    cartographer.mapping.proto.SerializedData serialized_data = 2;
  }
 }

--- a/cartographer/ground_truth/autogenerate_ground_truth_main.cc
+++ b/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;
-  {
-    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.";
-  }
+  mapping::proto::PoseGraph pose_graph =
+      io::DeserializePoseGraphFromFile(pose_graph_filename);
+
  LOG(INFO) << "Autogenerating ground truth relations...";
  const proto::GroundTruth ground_truth =
      GenerateGroundTruth(pose_graph, min_covered_distance,
--- a/cartographer/ground_truth/compute_relations_metrics_main.cc
+++ b/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;
-  {
-    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.";
-  }
+  mapping::proto::PoseGraph pose_graph =
+      io::DeserializePoseGraphFromFile(pose_graph_filename);
+
  const transform::TransformInterpolationBuffer transform_interpolation_buffer(
      pose_graph.trajectory(0));

--- a/cartographer/io/proto_stream_deserializer.cc
+++ b/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)) {
--- a/cartographer/io/proto_stream_deserializer.h
+++ b/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 {
--- a/cartographer/mapping/map_builder.cc
+++ b/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,
-    proto::SubmapQuery::Response* const response) {
+    const SubmapId& submap_id, 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.
-  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);
-      }
-    }
-  }
+  io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer);
 }

 void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
                           bool load_frozen_state) {
-  proto::PoseGraph pose_graph_proto;
-  CHECK(reader->ReadProto(&pose_graph_proto));
-  proto::AllTrajectoryBuilderOptions all_builder_options_proto;
-  CHECK(reader->ReadProto(&all_builder_options_proto));
-  CHECK_EQ(pose_graph_proto.trajectory_size(),
-           all_builder_options_proto.options_with_sensor_ids_size());
+  io::ProtoStreamDeserializer deserializer(reader);
+
+  // Create a copy of the pose_graph_proto, such that we can re-write the
+  // trajectory ids.
+  proto::PoseGraph pose_graph_proto = deserializer.pose_graph();
+  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 (;;) {
-    proto::LegacySerializedData proto;
-    if (!reader->ReadProto(&proto)) {
+  SerializedData proto;
+  while (deserializer.ReadNextSerializedData(&proto)) {
+    switch (proto.data_case()) {
+      case SerializedData::kPoseGraph:
+        LOG(ERROR) << "Found multiple serialized `PoseGraph`. Serialized "
+                      "stream likely corrupt!.";
+        break;
+      case SerializedData::kAllTrajectoryBuilderOptions:
+        LOG(ERROR) << "Found multiple serialized "
+                      "`AllTrajectoryBuilderOptions`. Serialized stream likely "
+                      "corrupt!.";
+        break;
+      case SerializedData::kSubmap: {
+        proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
+            trajectory_remapping.at(
+                proto.submap().submap_id().trajectory_id()));
+        const transform::Rigid3d& submap_pose = submap_poses.at(
+            SubmapId{proto.submap().submap_id().trajectory_id(),
+                     proto.submap().submap_id().submap_index()});
+        pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
        break;
      }
-    if (proto.has_node()) {
+      case SerializedData::kNode: {
        proto.mutable_node()->mutable_node_id()->set_trajectory_id(
            trajectory_remapping.at(proto.node().node_id().trajectory_id()));
-      const transform::Rigid3d node_pose =
+        const transform::Rigid3d& node_pose =
            node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
                                 proto.node().node_id().node_index()});
        pose_graph_->AddNodeFromProto(node_pose, proto.node());
+        break;
      }
-    if (proto.has_submap()) {
-      proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
-          trajectory_remapping.at(proto.submap().submap_id().trajectory_id()));
-      const transform::Rigid3d submap_pose =
-          submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(),
-                                   proto.submap().submap_id().submap_index()});
-      pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
-    }
-    if (proto.has_trajectory_data()) {
+      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;
      }
-    if (!load_frozen_state) {
-      if (proto.has_imu_data()) {
+      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(