Change pbstream migrate to support new submap format. (#1733)
This changes the migration support to: 1. Remove migration from old files from before version 1.0. If this is still needed by someone, they can use the tool as it was before this change. 2. Adds migration of maps without submap histograms. These were so far migrated on-the-fly, but #1710 aims to remove this. See also #1709. This was tested by converting a serialized 3D map and verifying it can still be used for localization. The file changed a little bit, but it seems to be good enough. Signed-off-by: Wolfgang Hess <whess@lyft.com>master
Wolfgang Hess
GitHub
parent
6c889490e2
commit
19a6eab07a
|
|
@ -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]
|
||||
<< " <input_filename> <output_filename> [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];
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,284 +18,198 @@
|
|||
|
||||
#include <vector>
|
||||
|
||||
#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<int, std::vector<SerializedData>>;
|
||||
|
||||
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<int> 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<common::ConfigurationFileResolver>(
|
||||
std::vector<std::string>{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<mapping::optimization::OptimizationProblem3D>(
|
||||
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<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>
|
||||
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<mapping::SubmapId, transform::Rigid3d> 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<mapping::NodeId, transform::Rigid3d> 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<mapping::SubmapId, mapping::proto::Submap>
|
||||
submap_id_to_submap;
|
||||
mapping::MapById<mapping::NodeId, mapping::proto::Node> 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<mapping::SubmapId, mapping::proto::Submap>
|
||||
|
|
|
|||
|
|
@ -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<mapping::SubmapId, mapping::proto::Submap>
|
||||
MigrateSubmapFormatVersion1ToVersion2(
|
||||
|
|
|
|||
|
|
@ -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 <class LegacySerializedDataType>
|
||||
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<mapping::proto::LegacySerializedData>(reader_);
|
||||
AddLegacyDataToReader<mapping::proto::LegacySerializedDataLegacySubmap>(
|
||||
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<ForwardingProtoStreamWriter> writer_;
|
||||
std::unique_ptr<ForwardingProtoStreamWriter> writer_for_migrating_grid_;
|
||||
std::vector<std::string> output_messages_;
|
||||
std::vector<std::string> 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<mapping::proto::SerializedData> 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<mapping::proto::SerializedData> 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<mapping::SubmapId, mapping::proto::Submap>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue