From a9c90da1a86f0192df6e4e787d83368bb9ddbe5d Mon Sep 17 00:00:00 2001 From: Sebastian Klose Date: Tue, 29 May 2018 16:56:18 +0200 Subject: [PATCH] Migration tool for serialization format (#1167) * New serialization protos * Moved old definition to legacy_serialized_data.proto * defining new serialization format as oneof. * Changing to legacy datatype * adding serialization migration * moving to io * adding serialization migration * moving to io * adding file for test * adding test * test for order or migrated serialized data * test for order or migrated serialized data * renaming tool * addressing comments * addressing more comments * minor polishing --- CMakeLists.txt | 5 + .../io/migrate_serialization_format_main.cc | 48 ++++++ .../io/serialization_format_migration.cc | 153 ++++++++++++++++++ .../io/serialization_format_migration.h | 36 +++++ .../io/serialization_format_migration_test.cc | 120 ++++++++++++++ 5 files changed, 362 insertions(+) create mode 100644 cartographer/io/migrate_serialization_format_main.cc create mode 100644 cartographer/io/serialization_format_migration.cc create mode 100644 cartographer/io/serialization_format_migration.h create mode 100644 cartographer/io/serialization_format_migration_test.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 4c0dc52..2e3a686 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -179,6 +179,11 @@ google_binary(cartographer_compute_relations_metrics cartographer/ground_truth/compute_relations_metrics_main.cc ) +google_binary(cartographer_migrate_serialization_format + SRCS + cartographer/io/migrate_serialization_format_main.cc +) + if(${BUILD_GRPC}) google_binary(cartographer_grpc_server SRCS diff --git a/cartographer/io/migrate_serialization_format_main.cc b/cartographer/io/migrate_serialization_format_main.cc new file mode 100644 index 0000000..802cd51 --- /dev/null +++ b/cartographer/io/migrate_serialization_format_main.cc @@ -0,0 +1,48 @@ +/* + * 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.h" +#include "cartographer/io/serialization_format_migration.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +DEFINE_string( + original_pbstream_file, "", + "Path to the pbstream file that will be migrated to the new version."); +DEFINE_string(output_pbstream_file, "", + "Output filename for the migrated pbstream."); + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + FLAGS_logtostderr = true; + google::SetUsageMessage( + "\n\n" + "Tool for migrating files that use the serialization output of " + "Cartographer 0.3, to the new serialization format, which includes a " + "header (Version 1)."); + google::ParseCommandLineFlags(&argc, &argv, true); + + if (FLAGS_original_pbstream_file.empty() || + FLAGS_output_pbstream_file.empty()) { + google::ShowUsageWithFlagsRestrict(argv[0], "migrate_serialization_format"); + return EXIT_FAILURE; + } + cartographer::io::ProtoStreamReader input(FLAGS_original_pbstream_file); + cartographer::io::ProtoStreamWriter output(FLAGS_output_pbstream_file); + cartographer::io::MigrateStreamFormatToVersion1(&input, &output); + + return EXIT_SUCCESS; +} diff --git a/cartographer/io/serialization_format_migration.cc b/cartographer/io/serialization_format_migration.cc new file mode 100644 index 0000000..7bb015c --- /dev/null +++ b/cartographer/io/serialization_format_migration.cc @@ -0,0 +1,153 @@ +/* + * 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/serialization_format_migration.h" + +#include +#include + +#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { +namespace { + +using mapping::proto::SerializedData; +using ProtoMap = std::unordered_map>; + +bool ReadPoseGraph(cartographer::io::ProtoStreamReaderInterface* const input, + ProtoMap* proto_map) { + auto& pose_graph_vec = (*proto_map)[SerializedData::kPoseGraph]; + pose_graph_vec.emplace_back(); + return input->ReadProto(pose_graph_vec.back().mutable_pose_graph()); +} + +bool ReadBuilderOptions( + cartographer::io::ProtoStreamReaderInterface* const input, + ProtoMap* proto_map) { + auto& options_vec = + (*proto_map)[SerializedData::kAllTrajectoryBuilderOptions]; + options_vec.emplace_back(); + return input->ReadProto( + options_vec.back().mutable_all_trajectory_builder_options()); +} + +bool DeserializeNext(cartographer::io::ProtoStreamReaderInterface* const input, + ProtoMap* proto_map) { + mapping::proto::LegacySerializedData legacy_data; + if (!input->ReadProto(&legacy_data)) return false; + + if (legacy_data.has_submap()) { + auto& output_vector = (*proto_map)[SerializedData::kSubmapFieldNumber]; + output_vector.emplace_back(); + *output_vector.back().mutable_submap() = legacy_data.submap(); + } + if (legacy_data.has_node()) { + auto& output_vector = (*proto_map)[SerializedData::kNodeFieldNumber]; + output_vector.emplace_back(); + *output_vector.back().mutable_node() = legacy_data.node(); + } + if (legacy_data.has_trajectory_data()) { + auto& output_vector = + (*proto_map)[SerializedData::kTrajectoryDataFieldNumber]; + output_vector.emplace_back(); + *output_vector.back().mutable_trajectory_data() = + legacy_data.trajectory_data(); + } + if (legacy_data.has_imu_data()) { + auto& output_vector = (*proto_map)[SerializedData::kImuDataFieldNumber]; + output_vector.emplace_back(); + *output_vector.back().mutable_imu_data() = legacy_data.imu_data(); + } + if (legacy_data.has_odometry_data()) { + auto& output_vector = (*proto_map)[SerializedData::kOdometryData]; + output_vector.emplace_back(); + *output_vector.back().mutable_odometry_data() = legacy_data.odometry_data(); + } + if (legacy_data.has_fixed_frame_pose_data()) { + auto& output_vector = + (*proto_map)[SerializedData::kFixedFramePoseDataFieldNumber]; + output_vector.emplace_back(); + *output_vector.back().mutable_fixed_frame_pose_data() = + legacy_data.fixed_frame_pose_data(); + } + if (legacy_data.has_landmark_data()) { + auto& output_vector = + (*proto_map)[SerializedData::kLandmarkDataFieldNumber]; + output_vector.emplace_back(); + *output_vector.back().mutable_landmark_data() = legacy_data.landmark_data(); + } + return true; +} + +ProtoMap ParseLegacyData( + cartographer::io::ProtoStreamReaderInterface* const input) { + ProtoMap proto_map; + CHECK(ReadPoseGraph(input, &proto_map)) + << "Input stream seems to differ from original stream format. Could " + "not " + "read PoseGraph as first message."; + CHECK(ReadBuilderOptions(input, &proto_map)) + << "Input stream seems to differ from original stream format. Could " + "not " + "read AllTrajectoryBuilderOptions as second message."; + do { + } while (DeserializeNext(input, &proto_map)); + return proto_map; +} + +mapping::proto::SerializationHeader CreateSerializationHeader() { + constexpr uint32_t kVersion1 = 1; + mapping::proto::SerializationHeader header; + header.set_format_version(kVersion1); + return header; +} + +void SerializeToVersion1Format( + const ProtoMap& deserialized_data, + cartographer::io::ProtoStreamWriterInterface* const output) { + const std::vector kFieldSerializationOrder = { + SerializedData::kPoseGraphFieldNumber, + SerializedData::kAllTrajectoryBuilderOptionsFieldNumber, + SerializedData::kSubmapFieldNumber, + SerializedData::kNodeFieldNumber, + SerializedData::kTrajectoryDataFieldNumber, + SerializedData::kImuDataFieldNumber, + SerializedData::kOdometryDataFieldNumber, + SerializedData::kFixedFramePoseDataFieldNumber, + SerializedData::kLandmarkDataFieldNumber}; + + output->WriteProto(CreateSerializationHeader()); + for (auto field_index : kFieldSerializationOrder) { + const auto proto_vector_it = deserialized_data.find(field_index); + if (proto_vector_it == deserialized_data.end()) continue; + for (const auto& proto : proto_vector_it->second) { + output->WriteProto(proto); + } + } +} +} // namespace + +void MigrateStreamFormatToVersion1( + cartographer::io::ProtoStreamReaderInterface* const input, + cartographer::io::ProtoStreamWriterInterface* const output) { + SerializeToVersion1Format(ParseLegacyData(input), output); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/serialization_format_migration.h b/cartographer/io/serialization_format_migration.h new file mode 100644 index 0000000..c6f4982 --- /dev/null +++ b/cartographer/io/serialization_format_migration.h @@ -0,0 +1,36 @@ +/* + * 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_SERIALIZATION_FORMAT_MIGRATION_H_ +#define CARTOGRAPHER_IO_SERIALIZATION_FORMAT_MIGRATION_H_ + +#include "cartographer/io/proto_stream_interface.h" + +namespace cartographer { +namespace io { + +// This helper function, migrates the input stream, which is supposed to match +// to the "old" stream format order (PoseGraph, AllTrajectoryBuilderOptions, +// SerializedData*) to the version 1 stream format (SerializationHeader, +// SerializedData*). +void MigrateStreamFormatToVersion1( + cartographer::io::ProtoStreamReaderInterface* const input, + cartographer::io::ProtoStreamWriterInterface* const output); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_SERIALIZATION_FORMAT_MIGRATION_H_ diff --git a/cartographer/io/serialization_format_migration_test.cc b/cartographer/io/serialization_format_migration_test.cc new file mode 100644 index 0000000..0d8bec3 --- /dev/null +++ b/cartographer/io/serialization_format_migration_test.cc @@ -0,0 +1,120 @@ +/* + * 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/serialization_format_migration.h" + +#include +#include + +#include "cartographer/io/internal/in_memory_proto_stream.h" +#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.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" +#include "gmock/gmock.h" +#include "google/protobuf/text_format.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace io { +namespace { + +using ::google::protobuf::TextFormat; +using ::testing::Eq; +using ::testing::SizeIs; + +class MigrationTest : public ::testing::Test { + protected: + void SetUp() { + writer_.reset(new ForwardingProtoStreamWriter( + [this](const google::protobuf::Message* proto) -> bool { + std::string msg_string; + TextFormat::PrintToString(*proto, &msg_string); + this->output_messages_.push_back(msg_string); + return true; + })); + + mapping::proto::PoseGraph pose_graph; + mapping::proto::AllTrajectoryBuilderOptions all_options; + mapping::proto::LegacySerializedData submap; + submap.mutable_submap(); + mapping::proto::LegacySerializedData node; + node.mutable_node(); + mapping::proto::LegacySerializedData imu_data; + imu_data.mutable_imu_data(); + mapping::proto::LegacySerializedData odometry_data; + odometry_data.mutable_odometry_data(); + mapping::proto::LegacySerializedData fixed_frame_pose; + fixed_frame_pose.mutable_fixed_frame_pose_data(); + mapping::proto::LegacySerializedData trajectory_data; + trajectory_data.mutable_trajectory_data(); + mapping::proto::LegacySerializedData landmark_data; + landmark_data.mutable_landmark_data(); + + reader_.AddProto(pose_graph); + reader_.AddProto(all_options); + reader_.AddProto(submap); + reader_.AddProto(node); + reader_.AddProto(imu_data); + reader_.AddProto(odometry_data); + reader_.AddProto(fixed_frame_pose); + reader_.AddProto(trajectory_data); + reader_.AddProto(landmark_data); + } + + InMemoryProtoStreamReader reader_; + std::unique_ptr writer_; + std::vector output_messages_; + + static constexpr int kNumOriginalMessages = 9; +}; + +TEST_F(MigrationTest, MigrationAddsHeaderAsFirstMessage) { + MigrateStreamFormatToVersion1(&reader_, writer_.get()); + // We expect one message more than the original number of messages, because of + // the added header. + EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1)); + + mapping::proto::SerializationHeader header; + EXPECT_TRUE(TextFormat::ParseFromString(output_messages_[0], &header)); + EXPECT_THAT(header.format_version(), Eq(1)); +} + +TEST_F(MigrationTest, SerializedDataOrderIsCorrect) { + MigrateStreamFormatToVersion1(&reader_, writer_.get()); + EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1)); + + std::vector serialized( + output_messages_.size() - 1); + for (size_t i = 1; i < output_messages_.size(); ++i) { + EXPECT_TRUE( + TextFormat::ParseFromString(output_messages_[i], &serialized[i - 1])); + } + + EXPECT_TRUE(serialized[0].has_pose_graph()); + EXPECT_TRUE(serialized[1].has_all_trajectory_builder_options()); + EXPECT_TRUE(serialized[2].has_submap()); + EXPECT_TRUE(serialized[3].has_node()); + EXPECT_TRUE(serialized[4].has_trajectory_data()); + EXPECT_TRUE(serialized[5].has_imu_data()); + EXPECT_TRUE(serialized[6].has_odometry_data()); + EXPECT_TRUE(serialized[7].has_fixed_frame_pose_data()); + EXPECT_TRUE(serialized[8].has_landmark_data()); +} + +} // namespace +} // namespace io +} // namespace cartographer