diff --git a/cartographer/io/internal/pbstream_migrate.cc b/cartographer/io/internal/pbstream_migrate.cc index a84af06..43d6b8a 100644 --- a/cartographer/io/internal/pbstream_migrate.cc +++ b/cartographer/io/internal/pbstream_migrate.cc @@ -21,19 +21,18 @@ #include "gflags/gflags.h" #include "glog/logging.h" -DEFINE_bool(migrate_grid_format, false, - "Set if the submap data of the input pbstream uses the old " - "probability grid format."); +DEFINE_bool(include_unfinished_submaps, true, + "Whether to include unfinished submaps in the output."); namespace cartographer { namespace io { int pbstream_migrate(int argc, char** argv) { std::stringstream ss; - ss << "\n\nTool for migrating files that use the serialization output of " - "Cartographer 0.3, to the new serialization format, which includes a " - "header (Version 1). You may need to specify the '--migrate_grid_format" - " flag if the input file contains submaps with the legacy grid format." + ss << "\n\nTool for migrating files that use submaps without histograms " + "to the new submap format, which includes a histogram. You can " + "set --include_unfinished_submaps to false if you want to exclude " + "unfinished submaps in the output." << "\nUsage: " << argv[0] << " " << argv[1] << " [flags]"; google::SetUsageMessage(ss.str()); @@ -44,10 +43,10 @@ int pbstream_migrate(int argc, char** argv) { } cartographer::io::ProtoStreamReader input(argv[2]); cartographer::io::ProtoStreamWriter output(argv[3]); - LOG(INFO) << "Migrating old serialization format in \"" << argv[2] - << "\" to new serialization format in \"" << argv[3] << "\""; - cartographer::io::MigrateStreamFormatToVersion1(&input, &output, - FLAGS_migrate_grid_format); + LOG(INFO) << "Migrating serialization format 1 in \"" << argv[2] + << "\" to serialization format 2 in \"" << argv[3] << "\""; + cartographer::io::MigrateStreamVersion1ToVersion2( + &input, &output, FLAGS_include_unfinished_submaps); CHECK(output.Close()) << "Could not write migrated pbstream file to: " << argv[3]; diff --git a/cartographer/io/pbstream_main.cc b/cartographer/io/pbstream_main.cc index 0a795ff..4fffbd4 100644 --- a/cartographer/io/pbstream_main.cc +++ b/cartographer/io/pbstream_main.cc @@ -30,12 +30,12 @@ int main(int argc, char** argv) { "Swiss Army knife for pbstreams.\n\n" "Currently supported subcommands are:\n" "\tinfo - Prints summary of pbstream.\n" - "\tmigrate - Migrates old pbstream (w/o header) to new pbstream format."; + "\tmigrate - Migrates pbstream to the new submap format."; google::ParseCommandLineFlags(&argc, &argv, true); if (argc < 2) { google::SetUsageMessage(usage_message); - google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main"); + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_main"); return EXIT_FAILURE; } else if (std::string(argv[1]) == "info") { return ::cartographer::io::pbstream_info(argc, argv); @@ -44,7 +44,7 @@ int main(int argc, char** argv) { } else { LOG(INFO) << "Unknown subtool: \"" << argv[1]; google::SetUsageMessage(usage_message); - google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main"); + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_main"); return EXIT_FAILURE; } } diff --git a/cartographer/io/serialization_format_migration.cc b/cartographer/io/serialization_format_migration.cc index 186fb92..fb3c222 100644 --- a/cartographer/io/serialization_format_migration.cc +++ b/cartographer/io/serialization_format_migration.cc @@ -18,284 +18,198 @@ #include -#include "absl/container/flat_hash_map.h" +#include "cartographer/common/config.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/io/internal/mapping_state_serialization.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/internal/3d/pose_graph_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" +#include "cartographer/mapping/map_builder.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/probability_values.h" -#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h" -#include "cartographer/mapping/proto/internal/legacy_submap.pb.h" +#include "cartographer/mapping/proto/map_builder_options.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 = absl::flat_hash_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()); -} - -mapping::proto::Submap MigrateLegacySubmap2d( - const mapping::proto::LegacySubmap& submap_in) { - mapping::proto::Submap2D submap_2d; - - // Convert probability grid to generalized grid. - *submap_2d.mutable_grid()->mutable_limits() = - submap_in.submap_2d().probability_grid().limits(); - *submap_2d.mutable_grid()->mutable_cells() = - submap_in.submap_2d().probability_grid().cells(); - for (auto& cell : *submap_2d.mutable_grid()->mutable_cells()) { - cell = -1 * cell; - } - // CellBox can't be trivially copied because the protobuf - // serialization field number doesn't match. - submap_2d.mutable_grid()->mutable_known_cells_box()->set_max_x( - submap_in.submap_2d().probability_grid().known_cells_box().max_x()); - submap_2d.mutable_grid()->mutable_known_cells_box()->set_max_y( - submap_in.submap_2d().probability_grid().known_cells_box().max_y()); - submap_2d.mutable_grid()->mutable_known_cells_box()->set_min_x( - submap_in.submap_2d().probability_grid().known_cells_box().min_x()); - submap_2d.mutable_grid()->mutable_known_cells_box()->set_min_y( - submap_in.submap_2d().probability_grid().known_cells_box().min_y()); - - // Correspondence costs can be safely set to standard values. - // Otherwise, this would be done during the next deserialization, but with a - // warning, which we can avoid by setting it already here. - submap_2d.mutable_grid()->set_max_correspondence_cost( - mapping::kMaxCorrespondenceCost); - submap_2d.mutable_grid()->set_min_correspondence_cost( - mapping::kMinCorrespondenceCost); - submap_2d.mutable_grid()->mutable_probability_grid_2d(); - *submap_2d.mutable_local_pose() = submap_in.submap_2d().local_pose(); - submap_2d.set_num_range_data(submap_in.submap_2d().num_range_data()); - submap_2d.set_finished(submap_in.submap_2d().finished()); - - mapping::proto::Submap submap_out; - *submap_out.mutable_submap_2d() = submap_2d; - *submap_out.mutable_submap_id() = submap_in.submap_id(); - return submap_out; -} - -mapping::proto::Submap MigrateLegacySubmap3d( - const mapping::proto::LegacySubmap& submap_in) { - mapping::proto::Submap3D submap_3d; - *submap_3d.mutable_local_pose() = submap_in.submap_3d().local_pose(); - submap_3d.set_num_range_data(submap_in.submap_3d().num_range_data()); - submap_3d.set_finished(submap_in.submap_3d().finished()); - *submap_3d.mutable_high_resolution_hybrid_grid() = - submap_in.submap_3d().high_resolution_hybrid_grid(); - *submap_3d.mutable_low_resolution_hybrid_grid() = - submap_in.submap_3d().low_resolution_hybrid_grid(); - - mapping::proto::Submap submap_out; - *submap_out.mutable_submap_3d() = submap_3d; - *submap_out.mutable_submap_id() = submap_in.submap_id(); - return submap_out; -} - -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()) { - LOG_FIRST_N(INFO, 1) << "Migrating submap data."; - if (legacy_data.submap().has_submap_2d()) { - CHECK(legacy_data.submap().submap_2d().grid().has_probability_grid_2d() || - legacy_data.submap().submap_2d().grid().has_tsdf_2d()) - << "\nThe legacy data contains a 2D submap, but it's not using the " - "expected grid format. Try to migrate the grid format instead."; - } - auto& output_vector = (*proto_map)[SerializedData::kSubmapFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_submap() = legacy_data.submap(); - } - if (legacy_data.has_node()) { - LOG_FIRST_N(INFO, 1) << "Migrating node data."; - 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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; -} - -bool DeserializeNextAndMigrateGridFormat( - cartographer::io::ProtoStreamReaderInterface* const input, - ProtoMap* proto_map) { - mapping::proto::LegacySerializedDataLegacySubmap legacy_data; - if (!input->ReadProto(&legacy_data)) return false; - - if (legacy_data.has_submap()) { - LOG_FIRST_N(INFO, 1) << "Migrating submap data."; - auto& output_vector = (*proto_map)[SerializedData::kSubmapFieldNumber]; - output_vector.emplace_back(); - if (legacy_data.submap().has_submap_2d()) { - CHECK(legacy_data.submap().submap_2d().has_probability_grid()) - << "\nThe legacy data contains a 2D submap, but it has no legacy " - "probability grid that can be migrated to the new grid format."; - *output_vector.back().mutable_submap() = - MigrateLegacySubmap2d(legacy_data.submap()); - } else { - *output_vector.back().mutable_submap() = - MigrateLegacySubmap3d(legacy_data.submap()); - } - } - if (legacy_data.has_node()) { - LOG_FIRST_N(INFO, 1) << "Migrating node data."; - 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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()) { - LOG_FIRST_N(INFO, 1) << "Migrating 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, - bool migrate_grid_format) { - 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."; - if (migrate_grid_format) { - do { - } while (DeserializeNextAndMigrateGridFormat(input, &proto_map)); - } else { - 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}; - - LOG(INFO) << "Writing proto stream."; - 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( +void MigrateStreamVersion1ToVersion2( cartographer::io::ProtoStreamReaderInterface* const input, cartographer::io::ProtoStreamWriterInterface* const output, - bool migrate_grid_format) { - SerializeToVersion1Format(ParseLegacyData(input, migrate_grid_format), - output); + bool include_unfinished_submaps) { + auto file_resolver = ::absl::make_unique( + std::vector{std::string(common::kSourceDirectory) + + "/configuration_files"}); + const std::string kCode = R"text( + include "map_builder.lua" + MAP_BUILDER.use_trajectory_builder_3d = true + return MAP_BUILDER)text"; + cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + kCode, std::move(file_resolver)); + const auto options = + mapping::CreateMapBuilderOptions(&lua_parameter_dictionary); + + common::ThreadPool thread_pool(1); + CHECK(!options.use_trajectory_builder_2d()); + // We always use 3D here. 2D submaps do not have histograms. + mapping::PoseGraph3D pose_graph( + options.pose_graph_options(), + absl::make_unique( + options.pose_graph_options().optimization_problem_options()), + &thread_pool); + + ProtoStreamDeserializer deserializer(input); + + // Create a copy of the pose_graph_proto, such that we can re-write the + // trajectory ids. + mapping::proto::PoseGraph pose_graph_proto = deserializer.pose_graph(); + const auto& all_builder_options_proto = + deserializer.all_trajectory_builder_options(); + + std::vector + trajectory_builder_options; + for (int i = 0; i < pose_graph_proto.trajectory_size(); ++i) { + auto& trajectory_proto = *pose_graph_proto.mutable_trajectory(i); + const auto& options_with_sensor_ids_proto = + all_builder_options_proto.options_with_sensor_ids(i); + trajectory_builder_options.push_back(options_with_sensor_ids_proto); + CHECK_EQ(trajectory_proto.trajectory_id(), i); + } + + // Apply the calculated remapping to constraints in the pose graph proto. + for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) { + constraint_proto.mutable_submap_id()->set_trajectory_id( + constraint_proto.submap_id().trajectory_id()); + constraint_proto.mutable_node_id()->set_trajectory_id( + constraint_proto.node_id().trajectory_id()); + } + + mapping::MapById submap_poses; + for (const mapping::proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const mapping::proto::Trajectory::Submap& submap_proto : + trajectory_proto.submap()) { + submap_poses.Insert(mapping::SubmapId{trajectory_proto.trajectory_id(), + submap_proto.submap_index()}, + transform::ToRigid3(submap_proto.pose())); + } + } + + mapping::MapById node_poses; + for (const mapping::proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const mapping::proto::Trajectory::Node& node_proto : + trajectory_proto.node()) { + node_poses.Insert(mapping::NodeId{trajectory_proto.trajectory_id(), + node_proto.node_index()}, + transform::ToRigid3(node_proto.pose())); + } + } + + // Set global poses of landmarks. + for (const auto& landmark : pose_graph_proto.landmark_poses()) { + pose_graph.SetLandmarkPose(landmark.landmark_id(), + transform::ToRigid3(landmark.global_pose()), + true); + } + + mapping::MapById + submap_id_to_submap; + mapping::MapById node_id_to_node; + SerializedData proto; + while (deserializer.ReadNextSerializedData(&proto)) { + switch (proto.data_case()) { + case SerializedData::kPoseGraph: + LOG(FATAL) << "Found multiple serialized `PoseGraph`. Serialized " + "stream likely corrupt!."; + case SerializedData::kAllTrajectoryBuilderOptions: + LOG(FATAL) << "Found multiple serialized " + "`AllTrajectoryBuilderOptions`. Serialized stream likely " + "corrupt!."; + case SerializedData::kSubmap: { + CHECK(proto.submap().has_submap_3d()) + << "Converting to the new submap format only makes sense for 3D."; + proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( + proto.submap().submap_id().trajectory_id()); + submap_id_to_submap.Insert( + mapping::SubmapId{proto.submap().submap_id().trajectory_id(), + proto.submap().submap_id().submap_index()}, + proto.submap()); + break; + } + case SerializedData::kNode: { + proto.mutable_node()->mutable_node_id()->set_trajectory_id( + proto.node().node_id().trajectory_id()); + const mapping::NodeId node_id(proto.node().node_id().trajectory_id(), + proto.node().node_id().node_index()); + const transform::Rigid3d& node_pose = node_poses.at(node_id); + pose_graph.AddNodeFromProto(node_pose, proto.node()); + node_id_to_node.Insert(node_id, proto.node()); + break; + } + case SerializedData::kTrajectoryData: { + proto.mutable_trajectory_data()->set_trajectory_id( + proto.trajectory_data().trajectory_id()); + pose_graph.SetTrajectoryDataFromProto(proto.trajectory_data()); + break; + } + case SerializedData::kImuData: { + pose_graph.AddImuData(proto.imu_data().trajectory_id(), + sensor::FromProto(proto.imu_data().imu_data())); + break; + } + case SerializedData::kOdometryData: { + pose_graph.AddOdometryData( + proto.odometry_data().trajectory_id(), + sensor::FromProto(proto.odometry_data().odometry_data())); + break; + } + case SerializedData::kFixedFramePoseData: { + pose_graph.AddFixedFramePoseData( + proto.fixed_frame_pose_data().trajectory_id(), + sensor::FromProto( + proto.fixed_frame_pose_data().fixed_frame_pose_data())); + break; + } + case SerializedData::kLandmarkData: { + pose_graph.AddLandmarkData( + proto.landmark_data().trajectory_id(), + sensor::FromProto(proto.landmark_data().landmark_data())); + break; + } + default: + LOG(WARNING) << "Skipping unknown message type in stream: " + << proto.GetTypeName(); + } + } + + // TODO(schwoere): Remove backwards compatibility once the pbstream format + // version 2 is established. + if (deserializer.header().format_version() == + kFormatVersionWithoutSubmapHistograms) { + submap_id_to_submap = MigrateSubmapFormatVersion1ToVersion2( + submap_id_to_submap, node_id_to_node, pose_graph_proto); + } + + for (const auto& submap_id_submap : submap_id_to_submap) { + pose_graph.AddSubmapFromProto(submap_poses.at(submap_id_submap.id), + submap_id_submap.data); + } + + pose_graph.AddSerializedConstraints( + mapping::FromProto(pose_graph_proto.constraint())); + CHECK(input->eof()); + + WritePbStream(pose_graph, trajectory_builder_options, output, + include_unfinished_submaps); } mapping::MapById diff --git a/cartographer/io/serialization_format_migration.h b/cartographer/io/serialization_format_migration.h index 3570a8a..32e101d 100644 --- a/cartographer/io/serialization_format_migration.h +++ b/cartographer/io/serialization_format_migration.h @@ -24,14 +24,13 @@ 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( +// This helper function migrates the input stream, which is supposed +// to contain submaps without histograms (stream format version 1) to +// an output stream containing submaps with histograms (version 2). +void MigrateStreamVersion1ToVersion2( cartographer::io::ProtoStreamReaderInterface* const input, cartographer::io::ProtoStreamWriterInterface* const output, - bool migrate_grid_format); + bool include_unfinished_submaps); mapping::MapById MigrateSubmapFormatVersion1ToVersion2( diff --git a/cartographer/io/serialization_format_migration_test.cc b/cartographer/io/serialization_format_migration_test.cc index 16e29b2..19e3535 100644 --- a/cartographer/io/serialization_format_migration_test.cc +++ b/cartographer/io/serialization_format_migration_test.cc @@ -21,7 +21,6 @@ #include "cartographer/io/internal/in_memory_proto_stream.h" #include "cartographer/mapping/internal/testing/test_helpers.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" @@ -35,74 +34,6 @@ namespace cartographer { namespace io { namespace { -using ::google::protobuf::TextFormat; -using ::testing::Eq; -using ::testing::SizeIs; - -class MigrationTest : public ::testing::Test { - private: - template - void AddLegacyDataToReader(InMemoryProtoStreamReader& reader) { - mapping::proto::PoseGraph pose_graph; - mapping::proto::AllTrajectoryBuilderOptions all_options; - LegacySerializedDataType submap; - submap.mutable_submap(); - LegacySerializedDataType node; - node.mutable_node(); - LegacySerializedDataType imu_data; - imu_data.mutable_imu_data(); - LegacySerializedDataType odometry_data; - odometry_data.mutable_odometry_data(); - LegacySerializedDataType fixed_frame_pose; - fixed_frame_pose.mutable_fixed_frame_pose_data(); - LegacySerializedDataType trajectory_data; - trajectory_data.mutable_trajectory_data(); - LegacySerializedDataType 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); - } - - protected: - void SetUp() override { - AddLegacyDataToReader(reader_); - AddLegacyDataToReader( - reader_for_migrating_grid_); - - 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; - })); - writer_for_migrating_grid_.reset(new ForwardingProtoStreamWriter( - [this](const google::protobuf::Message* proto) -> bool { - std::string msg_string; - TextFormat::PrintToString(*proto, &msg_string); - this->output_messages_after_migrating_grid_.push_back(msg_string); - return true; - })); - } - - InMemoryProtoStreamReader reader_; - InMemoryProtoStreamReader reader_for_migrating_grid_; - std::unique_ptr writer_; - std::unique_ptr writer_for_migrating_grid_; - std::vector output_messages_; - std::vector output_messages_after_migrating_grid_; - - static constexpr int kNumOriginalMessages = 9; -}; - class SubmapHistogramMigrationTest : public ::testing::Test { protected: void SetUp() override { @@ -141,81 +72,6 @@ class SubmapHistogramMigrationTest : public ::testing::Test { mapping::proto::Node node_; }; -TEST_F(MigrationTest, MigrationAddsHeaderAsFirstMessage) { - MigrateStreamFormatToVersion1(&reader_, writer_.get(), - false /* migrate_grid_format */); - // 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(1u)); -} - -TEST_F(MigrationTest, MigrationWithGridMigrationAddsHeaderAsFirstMessage) { - MigrateStreamFormatToVersion1(&reader_for_migrating_grid_, - writer_for_migrating_grid_.get(), - true /* migrate_grid_format */); - // We expect one message more than the original number of messages, because of - // the added header. - EXPECT_THAT(output_messages_after_migrating_grid_, - SizeIs(kNumOriginalMessages + 1)); - - mapping::proto::SerializationHeader header; - EXPECT_TRUE(TextFormat::ParseFromString( - output_messages_after_migrating_grid_[0], &header)); - EXPECT_THAT(header.format_version(), Eq(1u)); -} - -TEST_F(MigrationTest, SerializedDataOrderIsCorrect) { - MigrateStreamFormatToVersion1(&reader_, writer_.get(), - false /* migrate_grid_format */); - 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()); -} - -TEST_F(MigrationTest, SerializedDataOrderAfterGridMigrationIsCorrect) { - MigrateStreamFormatToVersion1(&reader_for_migrating_grid_, - writer_for_migrating_grid_.get(), - true /* migrate_grid_format */); - EXPECT_THAT(output_messages_after_migrating_grid_, - SizeIs(kNumOriginalMessages + 1)); - - std::vector serialized( - output_messages_after_migrating_grid_.size() - 1); - for (size_t i = 1; i < output_messages_after_migrating_grid_.size(); ++i) { - EXPECT_TRUE(TextFormat::ParseFromString( - output_messages_after_migrating_grid_[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()); -} - TEST_F(SubmapHistogramMigrationTest, SubmapHistogramGenerationFromTrajectoryNodes) { mapping::MapById diff --git a/docs/source/pbstream_migration.rst b/docs/source/pbstream_migration.rst index f2699f3..20ce543 100644 --- a/docs/source/pbstream_migration.rst +++ b/docs/source/pbstream_migration.rst @@ -16,22 +16,30 @@ Migration tool for pbstream files ================================= -With the update of the pbstream serialization format as discussed in -`RFC-0021`_, previously serialized pbstream files are not loadable in -Cartographer 1.0 anymore. - -In order to enable users to reuse previously generated pbstream files, we -provide a migration tool which converts pbstreams from Cartographer 0.3 to the -new serialization format used in Cartographer 1.0. +The pbstream serialization format for 3D has changed to include additional +data (histograms) in each submap. Code to load old data by migrating +on-the-fly will be removed soon. Once this happened, users who wish to +migrate old pbstream files can use a migration tool. The tool is shipped as part of Cartographer's pbstream tool (`source`_) and once built can be invoked as follows::: cartographer_pbstream migrate old.pbstream new.pbstream -The tool assumes that the first pbstream provided as commandline argument, -follows the serialization format of Cartographer 0.3. The resulting -1.0 pbstream will be saved to the second commandline argument location. +The tool assumes 3D data in the old submap format as input and converts it +to the currently used format version. + +Migrating pre-1.0 pbstream files +================================ + +With the update of the pbstream serialization format as discussed in +`RFC-0021`_, previously serialized pbstream files are not loadable in +Cartographer 1.0 anymore. + +In order to enable users to reuse previously generated pbstream files, +migration using an older version of the migration tool is necessary. +The current tool does not support this migration anymore. Please use +the version at Git SHA 6c889490e245cc5d9da15023249c6fc7119def3f. .. _RFC-0021: https://github.com/cartographer-project/rfcs/blob/master/text/0021-serialization-format.md .. _source: https://github.com/cartographer-project/cartographer/blob/master/cartographer/io/pbstream_main.cc