Migrate pbstream files with old probability grid format. (#1314)
parent
52804df988
commit
4ba9d9168b
|
@ -21,6 +21,10 @@
|
||||||
#include "gflags/gflags.h"
|
#include "gflags/gflags.h"
|
||||||
#include "glog/logging.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.");
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace io {
|
namespace io {
|
||||||
|
|
||||||
|
@ -28,9 +32,10 @@ int pbstream_migrate(int argc, char** argv) {
|
||||||
std::stringstream ss;
|
std::stringstream ss;
|
||||||
ss << "\n\nTool for migrating files that use the serialization output of "
|
ss << "\n\nTool for migrating files that use the serialization output of "
|
||||||
"Cartographer 0.3, to the new serialization format, which includes a "
|
"Cartographer 0.3, to the new serialization format, which includes a "
|
||||||
"header (Version 1)."
|
"header (Version 1). You may need to specify the '--migrate_grid_format"
|
||||||
|
" flag if the input file contains submaps with the legacy grid format."
|
||||||
<< "\nUsage: " << argv[0] << " " << argv[1]
|
<< "\nUsage: " << argv[0] << " " << argv[1]
|
||||||
<< " <input_filename> <output_filename>";
|
<< " <input_filename> <output_filename> [flags]";
|
||||||
google::SetUsageMessage(ss.str());
|
google::SetUsageMessage(ss.str());
|
||||||
|
|
||||||
if (argc < 4) {
|
if (argc < 4) {
|
||||||
|
@ -41,7 +46,8 @@ int pbstream_migrate(int argc, char** argv) {
|
||||||
cartographer::io::ProtoStreamWriter output(argv[3]);
|
cartographer::io::ProtoStreamWriter output(argv[3]);
|
||||||
LOG(INFO) << "Migrating old serialization format in \"" << argv[2]
|
LOG(INFO) << "Migrating old serialization format in \"" << argv[2]
|
||||||
<< "\" to new serialization format in \"" << argv[3] << "\"";
|
<< "\" to new serialization format in \"" << argv[3] << "\"";
|
||||||
cartographer::io::MigrateStreamFormatToVersion1(&input, &output);
|
cartographer::io::MigrateStreamFormatToVersion1(&input, &output,
|
||||||
|
FLAGS_migrate_grid_format);
|
||||||
|
|
||||||
return EXIT_SUCCESS;
|
return EXIT_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
|
@ -26,14 +26,15 @@ int main(int argc, char** argv) {
|
||||||
google::InitGoogleLogging(argv[0]);
|
google::InitGoogleLogging(argv[0]);
|
||||||
|
|
||||||
FLAGS_logtostderr = true;
|
FLAGS_logtostderr = true;
|
||||||
google::SetUsageMessage(
|
const std::string usage_message =
|
||||||
"Swiss Army knife for pbstreams.\n\n"
|
"Swiss Army knife for pbstreams.\n\n"
|
||||||
"Currently supported subcommands are:\n"
|
"Currently supported subcommands are:\n"
|
||||||
"\tinfo - Prints summary of pbstream.\n"
|
"\tinfo - Prints summary of pbstream.\n"
|
||||||
"\tmigrate - Migrates old pbstream (w/o header) to new pbstream format.");
|
"\tmigrate - Migrates old pbstream (w/o header) to new pbstream format.";
|
||||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||||
|
|
||||||
if (argc < 2) {
|
if (argc < 2) {
|
||||||
|
google::SetUsageMessage(usage_message);
|
||||||
google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main");
|
google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main");
|
||||||
return EXIT_FAILURE;
|
return EXIT_FAILURE;
|
||||||
} else if (std::string(argv[1]) == "info") {
|
} else if (std::string(argv[1]) == "info") {
|
||||||
|
@ -42,6 +43,7 @@ int main(int argc, char** argv) {
|
||||||
return ::cartographer::io::pbstream_migrate(argc, argv);
|
return ::cartographer::io::pbstream_migrate(argc, argv);
|
||||||
} else {
|
} else {
|
||||||
LOG(INFO) << "Unknown subtool: \"" << argv[1];
|
LOG(INFO) << "Unknown subtool: \"" << argv[1];
|
||||||
|
google::SetUsageMessage(usage_message);
|
||||||
google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main");
|
google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main");
|
||||||
return EXIT_FAILURE;
|
return EXIT_FAILURE;
|
||||||
}
|
}
|
||||||
|
|
|
@ -19,7 +19,9 @@
|
||||||
#include <unordered_map>
|
#include <unordered_map>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
|
#include "cartographer/mapping/probability_values.h"
|
||||||
#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h"
|
#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h"
|
||||||
|
#include "cartographer/mapping/proto/internal/legacy_submap.pb.h"
|
||||||
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
|
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
|
|
||||||
|
@ -47,22 +49,89 @@ bool ReadBuilderOptions(
|
||||||
options_vec.back().mutable_all_trajectory_builder_options());
|
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,
|
bool DeserializeNext(cartographer::io::ProtoStreamReaderInterface* const input,
|
||||||
ProtoMap* proto_map) {
|
ProtoMap* proto_map) {
|
||||||
mapping::proto::LegacySerializedData legacy_data;
|
mapping::proto::LegacySerializedData legacy_data;
|
||||||
if (!input->ReadProto(&legacy_data)) return false;
|
if (!input->ReadProto(&legacy_data)) return false;
|
||||||
|
|
||||||
if (legacy_data.has_submap()) {
|
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];
|
auto& output_vector = (*proto_map)[SerializedData::kSubmapFieldNumber];
|
||||||
output_vector.emplace_back();
|
output_vector.emplace_back();
|
||||||
*output_vector.back().mutable_submap() = legacy_data.submap();
|
*output_vector.back().mutable_submap() = legacy_data.submap();
|
||||||
}
|
}
|
||||||
if (legacy_data.has_node()) {
|
if (legacy_data.has_node()) {
|
||||||
|
LOG_FIRST_N(INFO, 1) << "Migrating node data.";
|
||||||
auto& output_vector = (*proto_map)[SerializedData::kNodeFieldNumber];
|
auto& output_vector = (*proto_map)[SerializedData::kNodeFieldNumber];
|
||||||
output_vector.emplace_back();
|
output_vector.emplace_back();
|
||||||
*output_vector.back().mutable_node() = legacy_data.node();
|
*output_vector.back().mutable_node() = legacy_data.node();
|
||||||
}
|
}
|
||||||
if (legacy_data.has_trajectory_data()) {
|
if (legacy_data.has_trajectory_data()) {
|
||||||
|
LOG_FIRST_N(INFO, 1) << "Migrating trajectory data.";
|
||||||
auto& output_vector =
|
auto& output_vector =
|
||||||
(*proto_map)[SerializedData::kTrajectoryDataFieldNumber];
|
(*proto_map)[SerializedData::kTrajectoryDataFieldNumber];
|
||||||
output_vector.emplace_back();
|
output_vector.emplace_back();
|
||||||
|
@ -70,16 +139,19 @@ bool DeserializeNext(cartographer::io::ProtoStreamReaderInterface* const input,
|
||||||
legacy_data.trajectory_data();
|
legacy_data.trajectory_data();
|
||||||
}
|
}
|
||||||
if (legacy_data.has_imu_data()) {
|
if (legacy_data.has_imu_data()) {
|
||||||
|
LOG_FIRST_N(INFO, 1) << "Migrating IMU data.";
|
||||||
auto& output_vector = (*proto_map)[SerializedData::kImuDataFieldNumber];
|
auto& output_vector = (*proto_map)[SerializedData::kImuDataFieldNumber];
|
||||||
output_vector.emplace_back();
|
output_vector.emplace_back();
|
||||||
*output_vector.back().mutable_imu_data() = legacy_data.imu_data();
|
*output_vector.back().mutable_imu_data() = legacy_data.imu_data();
|
||||||
}
|
}
|
||||||
if (legacy_data.has_odometry_data()) {
|
if (legacy_data.has_odometry_data()) {
|
||||||
|
LOG_FIRST_N(INFO, 1) << "Migrating odometry data.";
|
||||||
auto& output_vector = (*proto_map)[SerializedData::kOdometryData];
|
auto& output_vector = (*proto_map)[SerializedData::kOdometryData];
|
||||||
output_vector.emplace_back();
|
output_vector.emplace_back();
|
||||||
*output_vector.back().mutable_odometry_data() = legacy_data.odometry_data();
|
*output_vector.back().mutable_odometry_data() = legacy_data.odometry_data();
|
||||||
}
|
}
|
||||||
if (legacy_data.has_fixed_frame_pose_data()) {
|
if (legacy_data.has_fixed_frame_pose_data()) {
|
||||||
|
LOG_FIRST_N(INFO, 1) << "Migrating fixed frame pose data.";
|
||||||
auto& output_vector =
|
auto& output_vector =
|
||||||
(*proto_map)[SerializedData::kFixedFramePoseDataFieldNumber];
|
(*proto_map)[SerializedData::kFixedFramePoseDataFieldNumber];
|
||||||
output_vector.emplace_back();
|
output_vector.emplace_back();
|
||||||
|
@ -87,6 +159,72 @@ bool DeserializeNext(cartographer::io::ProtoStreamReaderInterface* const input,
|
||||||
legacy_data.fixed_frame_pose_data();
|
legacy_data.fixed_frame_pose_data();
|
||||||
}
|
}
|
||||||
if (legacy_data.has_landmark_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 =
|
auto& output_vector =
|
||||||
(*proto_map)[SerializedData::kLandmarkDataFieldNumber];
|
(*proto_map)[SerializedData::kLandmarkDataFieldNumber];
|
||||||
output_vector.emplace_back();
|
output_vector.emplace_back();
|
||||||
|
@ -96,7 +234,8 @@ bool DeserializeNext(cartographer::io::ProtoStreamReaderInterface* const input,
|
||||||
}
|
}
|
||||||
|
|
||||||
ProtoMap ParseLegacyData(
|
ProtoMap ParseLegacyData(
|
||||||
cartographer::io::ProtoStreamReaderInterface* const input) {
|
cartographer::io::ProtoStreamReaderInterface* const input,
|
||||||
|
bool migrate_grid_format) {
|
||||||
ProtoMap proto_map;
|
ProtoMap proto_map;
|
||||||
CHECK(ReadPoseGraph(input, &proto_map))
|
CHECK(ReadPoseGraph(input, &proto_map))
|
||||||
<< "Input stream seems to differ from original stream format. Could "
|
<< "Input stream seems to differ from original stream format. Could "
|
||||||
|
@ -106,8 +245,13 @@ ProtoMap ParseLegacyData(
|
||||||
<< "Input stream seems to differ from original stream format. Could "
|
<< "Input stream seems to differ from original stream format. Could "
|
||||||
"not "
|
"not "
|
||||||
"read AllTrajectoryBuilderOptions as second message.";
|
"read AllTrajectoryBuilderOptions as second message.";
|
||||||
|
if (migrate_grid_format) {
|
||||||
|
do {
|
||||||
|
} while (DeserializeNextAndMigrateGridFormat(input, &proto_map));
|
||||||
|
} else {
|
||||||
do {
|
do {
|
||||||
} while (DeserializeNext(input, &proto_map));
|
} while (DeserializeNext(input, &proto_map));
|
||||||
|
}
|
||||||
return proto_map;
|
return proto_map;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -145,8 +289,10 @@ void SerializeToVersion1Format(
|
||||||
|
|
||||||
void MigrateStreamFormatToVersion1(
|
void MigrateStreamFormatToVersion1(
|
||||||
cartographer::io::ProtoStreamReaderInterface* const input,
|
cartographer::io::ProtoStreamReaderInterface* const input,
|
||||||
cartographer::io::ProtoStreamWriterInterface* const output) {
|
cartographer::io::ProtoStreamWriterInterface* const output,
|
||||||
SerializeToVersion1Format(ParseLegacyData(input), output);
|
bool migrate_grid_format) {
|
||||||
|
SerializeToVersion1Format(ParseLegacyData(input, migrate_grid_format),
|
||||||
|
output);
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace io
|
} // namespace io
|
||||||
|
|
|
@ -28,7 +28,8 @@ namespace io {
|
||||||
// SerializedData*).
|
// SerializedData*).
|
||||||
void MigrateStreamFormatToVersion1(
|
void MigrateStreamFormatToVersion1(
|
||||||
cartographer::io::ProtoStreamReaderInterface* const input,
|
cartographer::io::ProtoStreamReaderInterface* const input,
|
||||||
cartographer::io::ProtoStreamWriterInterface* const output);
|
cartographer::io::ProtoStreamWriterInterface* const output,
|
||||||
|
bool migrate_grid_format);
|
||||||
|
|
||||||
} // namespace io
|
} // namespace io
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -37,8 +37,43 @@ using ::testing::Eq;
|
||||||
using ::testing::SizeIs;
|
using ::testing::SizeIs;
|
||||||
|
|
||||||
class MigrationTest : public ::testing::Test {
|
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:
|
protected:
|
||||||
void SetUp() {
|
void SetUp() {
|
||||||
|
AddLegacyDataToReader<mapping::proto::LegacySerializedData>(reader_);
|
||||||
|
AddLegacyDataToReader<mapping::proto::LegacySerializedDataLegacySubmap>(
|
||||||
|
reader_for_migrating_grid_);
|
||||||
|
|
||||||
writer_.reset(new ForwardingProtoStreamWriter(
|
writer_.reset(new ForwardingProtoStreamWriter(
|
||||||
[this](const google::protobuf::Message* proto) -> bool {
|
[this](const google::protobuf::Message* proto) -> bool {
|
||||||
std::string msg_string;
|
std::string msg_string;
|
||||||
|
@ -46,44 +81,28 @@ class MigrationTest : public ::testing::Test {
|
||||||
this->output_messages_.push_back(msg_string);
|
this->output_messages_.push_back(msg_string);
|
||||||
return true;
|
return true;
|
||||||
}));
|
}));
|
||||||
|
writer_for_migrating_grid_.reset(new ForwardingProtoStreamWriter(
|
||||||
mapping::proto::PoseGraph pose_graph;
|
[this](const google::protobuf::Message* proto) -> bool {
|
||||||
mapping::proto::AllTrajectoryBuilderOptions all_options;
|
std::string msg_string;
|
||||||
mapping::proto::LegacySerializedData submap;
|
TextFormat::PrintToString(*proto, &msg_string);
|
||||||
submap.mutable_submap();
|
this->output_messages_after_migrating_grid_.push_back(msg_string);
|
||||||
mapping::proto::LegacySerializedData node;
|
return true;
|
||||||
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_;
|
InMemoryProtoStreamReader reader_;
|
||||||
|
InMemoryProtoStreamReader reader_for_migrating_grid_;
|
||||||
std::unique_ptr<ForwardingProtoStreamWriter> writer_;
|
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_;
|
||||||
|
std::vector<std::string> output_messages_after_migrating_grid_;
|
||||||
|
|
||||||
static constexpr int kNumOriginalMessages = 9;
|
static constexpr int kNumOriginalMessages = 9;
|
||||||
};
|
};
|
||||||
|
|
||||||
TEST_F(MigrationTest, MigrationAddsHeaderAsFirstMessage) {
|
TEST_F(MigrationTest, MigrationAddsHeaderAsFirstMessage) {
|
||||||
MigrateStreamFormatToVersion1(&reader_, writer_.get());
|
MigrateStreamFormatToVersion1(&reader_, writer_.get(),
|
||||||
|
false /* migrate_grid_format */);
|
||||||
// We expect one message more than the original number of messages, because of
|
// We expect one message more than the original number of messages, because of
|
||||||
// the added header.
|
// the added header.
|
||||||
EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1));
|
EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1));
|
||||||
|
@ -93,8 +112,24 @@ TEST_F(MigrationTest, MigrationAddsHeaderAsFirstMessage) {
|
||||||
EXPECT_THAT(header.format_version(), Eq(1));
|
EXPECT_THAT(header.format_version(), Eq(1));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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(1));
|
||||||
|
}
|
||||||
|
|
||||||
TEST_F(MigrationTest, SerializedDataOrderIsCorrect) {
|
TEST_F(MigrationTest, SerializedDataOrderIsCorrect) {
|
||||||
MigrateStreamFormatToVersion1(&reader_, writer_.get());
|
MigrateStreamFormatToVersion1(&reader_, writer_.get(),
|
||||||
|
false /* migrate_grid_format */);
|
||||||
EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1));
|
EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1));
|
||||||
|
|
||||||
std::vector<mapping::proto::SerializedData> serialized(
|
std::vector<mapping::proto::SerializedData> serialized(
|
||||||
|
@ -115,6 +150,31 @@ TEST_F(MigrationTest, SerializedDataOrderIsCorrect) {
|
||||||
EXPECT_TRUE(serialized[8].has_landmark_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());
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace io
|
} // namespace io
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -24,9 +24,8 @@ namespace {
|
||||||
float MinCorrespondenceCostFromProto(const proto::Grid2D& proto) {
|
float MinCorrespondenceCostFromProto(const proto::Grid2D& proto) {
|
||||||
if (proto.min_correspondence_cost() == 0.f &&
|
if (proto.min_correspondence_cost() == 0.f &&
|
||||||
proto.max_correspondence_cost() == 0.f) {
|
proto.max_correspondence_cost() == 0.f) {
|
||||||
LOG(WARNING)
|
LOG(WARNING) << "proto::Grid2D: min_correspondence_cost "
|
||||||
<< "proto::Grid2D: max_correspondence_cost and min_correspondence_cost "
|
"is initialized with 0 indicating an older version of the "
|
||||||
"are initialized with 0 indicating an older version of the "
|
|
||||||
"protobuf format. Loading default values.";
|
"protobuf format. Loading default values.";
|
||||||
return kMinCorrespondenceCost;
|
return kMinCorrespondenceCost;
|
||||||
} else {
|
} else {
|
||||||
|
@ -37,9 +36,8 @@ float MinCorrespondenceCostFromProto(const proto::Grid2D& proto) {
|
||||||
float MaxCorrespondenceCostFromProto(const proto::Grid2D& proto) {
|
float MaxCorrespondenceCostFromProto(const proto::Grid2D& proto) {
|
||||||
if (proto.min_correspondence_cost() == 0.f &&
|
if (proto.min_correspondence_cost() == 0.f &&
|
||||||
proto.max_correspondence_cost() == 0.f) {
|
proto.max_correspondence_cost() == 0.f) {
|
||||||
LOG(WARNING)
|
LOG(WARNING) << "proto::Grid2D: max_correspondence_cost "
|
||||||
<< "proto::Grid2D: max_correspondence_cost and min_correspondence_cost "
|
"is initialized with 0 indicating an older version of the "
|
||||||
"are initialized with 0 indicating an older version of the "
|
|
||||||
"protobuf format. Loading default values.";
|
"protobuf format. Loading default values.";
|
||||||
return kMaxCorrespondenceCost;
|
return kMaxCorrespondenceCost;
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -0,0 +1,37 @@
|
||||||
|
// Copyright 2016 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.
|
||||||
|
|
||||||
|
syntax = "proto3";
|
||||||
|
|
||||||
|
package cartographer.mapping.proto;
|
||||||
|
|
||||||
|
import "cartographer/mapping/proto/2d/map_limits.proto";
|
||||||
|
|
||||||
|
// The legacy probability grid that was used before the generalized
|
||||||
|
// grid structure was introduced.
|
||||||
|
|
||||||
|
message LegacyProbabilityGrid {
|
||||||
|
message CellBox {
|
||||||
|
int32 max_x = 1;
|
||||||
|
int32 max_y = 2;
|
||||||
|
int32 min_x = 3;
|
||||||
|
int32 min_y = 4;
|
||||||
|
}
|
||||||
|
|
||||||
|
MapLimits limits = 1;
|
||||||
|
// These values are actually int16s, but protos don't have a native int16
|
||||||
|
// type.
|
||||||
|
repeated int32 cells = 2;
|
||||||
|
CellBox known_cells_box = 8;
|
||||||
|
}
|
|
@ -17,6 +17,7 @@ syntax = "proto3";
|
||||||
package cartographer.mapping.proto;
|
package cartographer.mapping.proto;
|
||||||
|
|
||||||
import "cartographer/mapping/proto/serialization.proto";
|
import "cartographer/mapping/proto/serialization.proto";
|
||||||
|
import "cartographer/mapping/proto/internal/legacy_submap.proto";
|
||||||
|
|
||||||
message LegacySerializedData {
|
message LegacySerializedData {
|
||||||
Submap submap = 1;
|
Submap submap = 1;
|
||||||
|
@ -27,3 +28,15 @@ message LegacySerializedData {
|
||||||
TrajectoryData trajectory_data = 6;
|
TrajectoryData trajectory_data = 6;
|
||||||
LandmarkData landmark_data = 7;
|
LandmarkData landmark_data = 7;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// For backwards compatibility with serialized data containing the legacy
|
||||||
|
// submap format that did not yet use the generalized 2D grid format.
|
||||||
|
message LegacySerializedDataLegacySubmap {
|
||||||
|
LegacySubmap submap = 1;
|
||||||
|
Node node = 2;
|
||||||
|
ImuData imu_data = 3;
|
||||||
|
OdometryData odometry_data = 4;
|
||||||
|
FixedFramePoseData fixed_frame_pose_data = 5;
|
||||||
|
TrajectoryData trajectory_data = 6;
|
||||||
|
LandmarkData landmark_data = 7;
|
||||||
|
}
|
||||||
|
|
|
@ -0,0 +1,46 @@
|
||||||
|
// Copyright 2016 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.
|
||||||
|
|
||||||
|
syntax = "proto3";
|
||||||
|
|
||||||
|
package cartographer.mapping.proto;
|
||||||
|
|
||||||
|
import "cartographer/mapping/proto/pose_graph.proto";
|
||||||
|
import "cartographer/mapping/proto/internal/legacy_probability_grid.proto";
|
||||||
|
import "cartographer/mapping/proto/3d/hybrid_grid.proto";
|
||||||
|
import "cartographer/transform/proto/transform.proto";
|
||||||
|
|
||||||
|
// Serialized state of a Submap2D.
|
||||||
|
// Uses the legacy, non-generalized probability grid format.
|
||||||
|
message LegacySubmap2D {
|
||||||
|
transform.proto.Rigid3d local_pose = 1;
|
||||||
|
int32 num_range_data = 2;
|
||||||
|
bool finished = 3;
|
||||||
|
LegacyProbabilityGrid probability_grid = 4;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Serialized state of a Submap3D.
|
||||||
|
message LegacySubmap3D {
|
||||||
|
transform.proto.Rigid3d local_pose = 1;
|
||||||
|
int32 num_range_data = 2;
|
||||||
|
bool finished = 3;
|
||||||
|
HybridGrid high_resolution_hybrid_grid = 4;
|
||||||
|
HybridGrid low_resolution_hybrid_grid = 5;
|
||||||
|
}
|
||||||
|
|
||||||
|
message LegacySubmap {
|
||||||
|
SubmapId submap_id = 1;
|
||||||
|
LegacySubmap2D submap_2d = 2;
|
||||||
|
LegacySubmap3D submap_3d = 3;
|
||||||
|
}
|
Loading…
Reference in New Issue