Mapping state deserializer (#1171)

class for de-serializing mapping state stored in the new serialization format [RFC 0021](https://github.com/sebastianklose/rfcs/blob/serialization_format/text/0021-serialization-format.md).

Not used yet. Will be hooked up in a separate PR that switches all logic to the new serialization format at once.

cartographer/cloud/internal/client_server_test.cc

@@ -129,13 +129,9 @@ class ClientServerTest : public ::testing::Test {
     auto mock_map_builder = common::make_unique<MockMapBuilder>();
     mock_map_builder_ = mock_map_builder.get();
     mock_pose_graph_ = common::make_unique<MockPoseGraph>();
-    EXPECT_CALL(
+    EXPECT_CALL(*mock_map_builder_, pose_graph())
-        *mock_map_builder_,
-        pose_graph())
         .WillOnce(::testing::Return(mock_pose_graph_.get()));
-    EXPECT_CALL(
+    EXPECT_CALL(*mock_pose_graph_, SetGlobalSlamOptimizationCallback(_));
-        *mock_pose_graph_,
-        SetGlobalSlamOptimizationCallback(_));
     server_ = common::make_unique<MapBuilderServer>(
         map_builder_server_options_, std::move(mock_map_builder));
     EXPECT_TRUE(server_ != nullptr);

cartographer/io/proto_stream_deserializer.cc

@@ -0,0 +1,70 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/io/proto_stream_deserializer.h"
+
+#include "cartographer/io/internal/mapping_state_serialization.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+namespace {
+mapping::proto::SerializationHeader ReadHeaderOrDie(
+    ProtoStreamReaderInterface* const reader) {
+  mapping::proto::SerializationHeader header;
+  CHECK(reader->ReadProto(&header)) << "Failed to read SerializationHeader.";
+  return header;
+}
+
+bool IsVersionSupported(const mapping::proto::SerializationHeader& header) {
+  return header.format_version() == kMappingStateSerializationFormatVersion;
+}
+
+}  // namespace
+
+ProtoStreamDeserializer::ProtoStreamDeserializer(
+    ProtoStreamReaderInterface* const reader)
+    : reader_(reader), header_(ReadHeaderOrDie(reader)) {
+  CHECK(IsVersionSupported(header_)) << "Unsupported serialization format \""
+                                     << header_.format_version() << "\"";
+
+  CHECK(ReadNextSerializedData(&pose_graph_))
+      << "Serialized stream misses PoseGraph.";
+  CHECK(pose_graph_.has_pose_graph())
+      << "Serialized stream order corrupt. Expecting `PoseGraph` after "
+         "`SerializationHeader`, but got field tag "
+      << pose_graph_.data_case();
+
+  CHECK(ReadNextSerializedData(&all_trajectory_builder_options_))
+      << "Serialized stream misses `AllTrajectoryBuilderOptions`.";
+  CHECK(all_trajectory_builder_options_.has_all_trajectory_builder_options())
+      << "Serialized stream order corrupt. Expecting "
+         "`AllTrajectoryBuilderOptions` after "
+         "PoseGraph, got field tag "
+      << all_trajectory_builder_options_.data_case();
+
+  CHECK_EQ(pose_graph_.pose_graph().trajectory_size(),
+           all_trajectory_builder_options_.all_trajectory_builder_options()
+               .options_with_sensor_ids_size());
+}
+
+bool ProtoStreamDeserializer::ReadNextSerializedData(
+    mapping::proto::SerializedData* data) {
+  return reader_->ReadProto(data);
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/proto_stream_deserializer.h

@@ -0,0 +1,68 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_IO_PROTO_STREAM_DESERIALIZER_H_
+#define CARTOGRAPHER_IO_PROTO_STREAM_DESERIALIZER_H_
+
+#include "cartographer/io/proto_stream_interface.h"
+#include "cartographer/mapping/proto/pose_graph.pb.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
+#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
+
+namespace cartographer {
+namespace io {
+
+// Helper for deserializing a previously serialized mapping state from a
+// proto stream, abstracting away the format parsing logic.
+class ProtoStreamDeserializer {
+ public:
+  explicit ProtoStreamDeserializer(ProtoStreamReaderInterface* const reader);
+
+  ProtoStreamDeserializer(const ProtoStreamDeserializer&) = delete;
+  ProtoStreamDeserializer& operator=(const ProtoStreamDeserializer&) = delete;
+  ProtoStreamDeserializer(ProtoStreamDeserializer&&) = delete;
+
+  mapping::proto::SerializationHeader& header() { return header_; }
+
+  mapping::proto::PoseGraph& pose_graph() {
+    return *pose_graph_.mutable_pose_graph();
+  }
+  const mapping::proto::PoseGraph& pose_graph() const {
+    return pose_graph_.pose_graph();
+  }
+
+  const mapping::proto::AllTrajectoryBuilderOptions&
+  all_trajectory_builder_options() {
+    return all_trajectory_builder_options_.all_trajectory_builder_options();
+  }
+
+  // Reads the next `SerializedData` message of the ProtoStream into `data`.
+  // Returns `true` if the message was successfully read or `false` in case
+  // there are no-more messages or an error occurred.
+  bool ReadNextSerializedData(mapping::proto::SerializedData* data);
+
+ private:
+  ProtoStreamReaderInterface* reader_;
+
+  mapping::proto::SerializationHeader header_;
+  mapping::proto::SerializedData pose_graph_;
+  mapping::proto::SerializedData all_trajectory_builder_options_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_PROTO_STREAM_DESERIALIZER_H_

cartographer/io/proto_stream_deserializer_test.cc

@@ -0,0 +1,197 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <memory>
+
+#include "cartographer/io/internal/in_memory_proto_stream.h"
+#include "cartographer/io/proto_stream_deserializer.h"
+#include "glog/logging.h"
+#include "gmock/gmock.h"
+#include "google/protobuf/text_format.h"
+#include "google/protobuf/util/message_differencer.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace io {
+namespace {
+
+using ::cartographer::common::make_unique;
+using ::cartographer::mapping::proto::SerializationHeader;
+using ::cartographer::mapping::proto::SerializedData;
+using ::google::protobuf::Message;
+using ::google::protobuf::util::MessageDifferencer;
+using ::testing::Eq;
+using ::testing::Not;
+
+static constexpr char kSerializationHeaderProtoString[] = R"PROTO(
+  format_version: 1
+)PROTO";
+
+static constexpr char kUnsupportedSerializationHeaderProtoString[] = R"PROTO(
+  format_version: 123
+)PROTO";
+
+static constexpr char kPoseGraphProtoString[] = R"PROTO(
+  pose_graph {}
+)PROTO";
+
+static constexpr char kAllTrajectoryBuilderOptionsProtoString[] = R"PROTO(
+  all_trajectory_builder_options {}
+)PROTO";
+
+static constexpr char kSubmapProtoString[] = R"PROTO(
+  submap {}
+)PROTO";
+
+static constexpr char kNodeProtoString[] = R"PROTO(
+  node {}
+)PROTO";
+
+static constexpr char kTrajectoryDataProtoString[] = R"PROTO(
+  trajectory_data {}
+)PROTO";
+
+static constexpr char kImuDataProtoString[] = R"PROTO(
+  imu_data {}
+)PROTO";
+
+static constexpr char kOdometryDataProtoString[] = R"PROTO(
+  odometry_data {}
+)PROTO";
+
+static constexpr char kFixedFramePoseDataProtoString[] = R"PROTO(
+  fixed_frame_pose_data {}
+)PROTO";
+
+static constexpr char kLandmarkDataProtoString[] = R"PROTO(
+  landmark_data {}
+)PROTO";
+
+template <typename T>
+T ProtoFromStringOrDie(const std::string& proto_string) {
+  T msg;
+  CHECK(google::protobuf::TextFormat::ParseFromString(proto_string, &msg));
+  return msg;
+}
+
+template <typename T>
+std::unique_ptr<T> ProtoUPtrFromStringOrDie(const std::string& proto_string) {
+  return make_unique<T>(ProtoFromStringOrDie<T>(proto_string));
+}
+
+std::unique_ptr<InMemoryProtoStreamReader>
+CreateInMemoryReaderFromTextProtos() {
+  std::queue<std::unique_ptr<Message>> proto_queue;
+
+  proto_queue.emplace(ProtoUPtrFromStringOrDie<SerializationHeader>(
+      kSerializationHeaderProtoString));
+  proto_queue.emplace(
+      ProtoUPtrFromStringOrDie<SerializedData>(kPoseGraphProtoString));
+  proto_queue.emplace(ProtoUPtrFromStringOrDie<SerializedData>(
+      kAllTrajectoryBuilderOptionsProtoString));
+  proto_queue.emplace(
+      ProtoUPtrFromStringOrDie<SerializedData>(kSubmapProtoString));
+  proto_queue.emplace(
+      ProtoUPtrFromStringOrDie<SerializedData>(kNodeProtoString));
+  proto_queue.emplace(
+      ProtoUPtrFromStringOrDie<SerializedData>(kTrajectoryDataProtoString));
+  proto_queue.emplace(
+      ProtoUPtrFromStringOrDie<SerializedData>(kImuDataProtoString));
+  proto_queue.emplace(
+      ProtoUPtrFromStringOrDie<SerializedData>(kOdometryDataProtoString));
+  proto_queue.emplace(
+      ProtoUPtrFromStringOrDie<SerializedData>(kFixedFramePoseDataProtoString));
+  proto_queue.emplace(
+      ProtoUPtrFromStringOrDie<SerializedData>(kLandmarkDataProtoString));
+  return make_unique<InMemoryProtoStreamReader>(std::move(proto_queue));
+}
+
+// This test checks if the serialization works.
+TEST(ProtoStreamDeserializerTest, WorksOnGoldenTextStream) {
+  // Load text proto into in_memory_reader.
+  std::unique_ptr<InMemoryProtoStreamReader> reader =
+      CreateInMemoryReaderFromTextProtos();
+
+  io::ProtoStreamDeserializer deserializer(reader.get());
+
+  EXPECT_TRUE(MessageDifferencer::Equals(
+      deserializer.header(), ProtoFromStringOrDie<SerializationHeader>(
+                                 kSerializationHeaderProtoString)));
+
+  EXPECT_TRUE(MessageDifferencer::Equals(
+      deserializer.pose_graph(),
+      ProtoFromStringOrDie<SerializedData>(kPoseGraphProtoString)
+          .pose_graph()));
+
+  EXPECT_TRUE(
+      MessageDifferencer::Equals(deserializer.all_trajectory_builder_options(),
+                                 ProtoFromStringOrDie<SerializedData>(
+                                     kAllTrajectoryBuilderOptionsProtoString)
+                                     .all_trajectory_builder_options()));
+
+  SerializedData serialized_data;
+  EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
+  // TODO(sebastianklose): Add matcher for protos in common place and use here.
+  EXPECT_TRUE(MessageDifferencer::Equals(
+      serialized_data,
+      ProtoFromStringOrDie<SerializedData>(kSubmapProtoString)));
+
+  EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
+  EXPECT_TRUE(MessageDifferencer::Equals(
+      serialized_data, ProtoFromStringOrDie<SerializedData>(kNodeProtoString)));
+
+  EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
+  EXPECT_TRUE(MessageDifferencer::Equals(
+      serialized_data,
+      ProtoFromStringOrDie<SerializedData>(kTrajectoryDataProtoString)));
+
+  EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
+  EXPECT_TRUE(MessageDifferencer::Equals(
+      serialized_data,
+      ProtoFromStringOrDie<SerializedData>(kImuDataProtoString)));
+
+  EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
+  EXPECT_TRUE(MessageDifferencer::Equals(
+      serialized_data,
+      ProtoFromStringOrDie<SerializedData>(kOdometryDataProtoString)));
+
+  EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
+  EXPECT_TRUE(MessageDifferencer::Equals(
+      serialized_data,
+      ProtoFromStringOrDie<SerializedData>(kFixedFramePoseDataProtoString)));
+
+  EXPECT_TRUE(deserializer.ReadNextSerializedData(&serialized_data));
+  EXPECT_TRUE(MessageDifferencer::Equals(
+      serialized_data,
+      ProtoFromStringOrDie<SerializedData>(kLandmarkDataProtoString)));
+
+  EXPECT_FALSE(deserializer.ReadNextSerializedData(&serialized_data));
+  EXPECT_TRUE(reader->eof());
+}
+
+TEST(ProtoStreamDeserializerDeathTests, FailsIfVersionNotSupported) {
+  std::queue<std::unique_ptr<Message>> proto_queue;
+  proto_queue.emplace(ProtoUPtrFromStringOrDie<SerializationHeader>(
+      kUnsupportedSerializationHeaderProtoString));
+  InMemoryProtoStreamReader reader(std::move(proto_queue));
+
+  EXPECT_DEATH(common::make_unique<ProtoStreamDeserializer>(&reader),
+               "Unsupported serialization format");
+}
+
+}  // namespace
+}  // namespace io
+}  // namespace cartographer