From 73d18e5fc54bfa4e4e61fd7feb615b46834aa584 Mon Sep 17 00:00:00 2001 From: Sebastian Klose Date: Wed, 30 May 2018 13:31:33 +0200 Subject: [PATCH] Using new serialization format. (#1174) Updates everyone to use the new serialization format. A corresponding PR will be made in cartographer_ros for the various tools. --- cartographer/cloud/client/map_builder_stub.cc | 18 +- .../internal/handlers/write_state_handler.cc | 8 +- .../cloud/proto/map_builder_service.proto | 8 +- .../autogenerate_ground_truth_main.cc | 11 +- .../compute_relations_metrics_main.cc | 13 +- cartographer/io/proto_stream_deserializer.cc | 8 + cartographer/io/proto_stream_deserializer.h | 4 + cartographer/mapping/map_builder.cc | 262 ++++++------------ 8 files changed, 118 insertions(+), 214 deletions(-) diff --git a/cartographer/cloud/client/map_builder_stub.cc b/cartographer/cloud/client/map_builder_stub.cc index b29d836..c3c4e43 100644 --- a/cartographer/cloud/client/map_builder_stub.cc +++ b/cartographer/cloud/client/map_builder_stub.cc @@ -25,6 +25,7 @@ #include "cartographer/cloud/internal/handlers/write_state_handler.h" #include "cartographer/cloud/internal/sensor/serialization.h" #include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "glog/logging.h" namespace cartographer { @@ -111,11 +112,8 @@ void MapBuilderStub::SerializeState(io::ProtoStreamWriterInterface* writer) { proto::WriteStateResponse response; while (client.StreamRead(&response)) { switch (response.state_chunk_case()) { - case proto::WriteStateResponse::kPoseGraph: - writer->WriteProto(response.pose_graph()); - break; - case proto::WriteStateResponse::kAllTrajectoryBuilderOptions: - writer->WriteProto(response.all_trajectory_builder_options()); + case proto::WriteStateResponse::kHeader: + writer->WriteProto(response.header()); break; case proto::WriteStateResponse::kSerializedData: writer->WriteProto(response.serialized_data()); @@ -132,21 +130,25 @@ void MapBuilderStub::LoadState(io::ProtoStreamReaderInterface* reader, LOG(FATAL) << "Not implemented"; } async_grpc::Client client(client_channel_); + + io::ProtoStreamDeserializer deserializer(reader); // Request with a PoseGraph proto is sent first. { proto::LoadStateRequest request; - CHECK(reader->ReadProto(request.mutable_pose_graph())); + *request.mutable_pose_graph() = deserializer.pose_graph(); CHECK(client.Write(request)); } // Request with an AllTrajectoryBuilderOptions should be second. { proto::LoadStateRequest request; - CHECK(reader->ReadProto(request.mutable_all_trajectory_builder_options())); + *request.mutable_all_trajectory_builder_options() = + deserializer.all_trajectory_builder_options(); CHECK(client.Write(request)); } // Multiple requests with SerializedData are sent after. proto::LoadStateRequest request; - while (reader->ReadProto(request.mutable_serialized_data())) { + while ( + deserializer.ReadNextSerializedData(request.mutable_serialized_data())) { CHECK(client.Write(request)); } diff --git a/cartographer/cloud/internal/handlers/write_state_handler.cc b/cartographer/cloud/internal/handlers/write_state_handler.cc index 0ea8c23..1b480ff 100644 --- a/cartographer/cloud/internal/handlers/write_state_handler.cc +++ b/cartographer/cloud/internal/handlers/write_state_handler.cc @@ -36,11 +36,9 @@ void WriteStateHandler::OnRequest(const google::protobuf::Empty& request) { } auto response = common::make_unique(); - if (proto->GetTypeName() == "cartographer.mapping.proto.PoseGraph") { - response->mutable_pose_graph()->CopyFrom(*proto); - } else if (proto->GetTypeName() == - "cartographer.mapping.proto.AllTrajectoryBuilderOptions") { - response->mutable_all_trajectory_builder_options()->CopyFrom(*proto); + if (proto->GetTypeName() == + "cartographer.mapping.proto.SerializationHeader") { + response->mutable_header()->CopyFrom(*proto); } else if (proto->GetTypeName() == "cartographer.mapping.proto.SerializedData") { response->mutable_serialized_data()->CopyFrom(*proto); diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index 51fef3b..1f78fac 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/cartographer/cloud/proto/map_builder_service.proto @@ -185,10 +185,8 @@ message GetConstraintsResponse { message WriteStateResponse { oneof state_chunk { - cartographer.mapping.proto.PoseGraph pose_graph = 1; - cartographer.mapping.proto.AllTrajectoryBuilderOptions - all_trajectory_builder_options = 2; - cartographer.mapping.proto.SerializedData serialized_data = 3; + cartographer.mapping.proto.SerializationHeader header = 1; + cartographer.mapping.proto.SerializedData serialized_data = 2; } } @@ -280,7 +278,7 @@ service MapBuilderService { // Checks whether the trajectory is frozen. rpc IsTrajectoryFrozen(IsTrajectoryFrozenRequest) - returns (IsTrajectoryFrozenResponse); + returns (IsTrajectoryFrozenResponse); // Requests a PoseGraph to call RunFinalOptimization. rpc RunFinalOptimization(google.protobuf.Empty) diff --git a/cartographer/ground_truth/autogenerate_ground_truth_main.cc b/cartographer/ground_truth/autogenerate_ground_truth_main.cc index fd60b40..7caefe8 100644 --- a/cartographer/ground_truth/autogenerate_ground_truth_main.cc +++ b/cartographer/ground_truth/autogenerate_ground_truth_main.cc @@ -21,6 +21,7 @@ #include "cartographer/common/port.h" #include "cartographer/ground_truth/proto/relations.pb.h" #include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/transform/transform.h" #include "gflags/gflags.h" @@ -170,13 +171,9 @@ void Run(const std::string& pose_graph_filename, const double outlier_threshold_meters, const double outlier_threshold_radians) { LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'..."; - mapping::proto::PoseGraph pose_graph; - { - io::ProtoStreamReader reader(pose_graph_filename); - CHECK(reader.ReadProto(&pose_graph)); - CHECK_EQ(pose_graph.trajectory_size(), 1) - << "Only pose graphs containing a single trajectory are supported."; - } + mapping::proto::PoseGraph pose_graph = + io::DeserializePoseGraphFromFile(pose_graph_filename); + LOG(INFO) << "Autogenerating ground truth relations..."; const proto::GroundTruth ground_truth = GenerateGroundTruth(pose_graph, min_covered_distance, diff --git a/cartographer/ground_truth/compute_relations_metrics_main.cc b/cartographer/ground_truth/compute_relations_metrics_main.cc index fd520d1..0aba0b7 100644 --- a/cartographer/ground_truth/compute_relations_metrics_main.cc +++ b/cartographer/ground_truth/compute_relations_metrics_main.cc @@ -28,6 +28,7 @@ #include "cartographer/ground_truth/proto/relations.pb.h" #include "cartographer/ground_truth/relations_text_file.h" #include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -126,7 +127,7 @@ void WriteRelationMetricsToFile(const std::vector& errors, "expected_translation_x,expected_translation_y,expected_" "translation_z,expected_rotation_w,expected_rotation_x," "expected_rotation_y,expected_rotation_z,covered_distance\n"; - for (size_t relation_index = 0; relation_index < ground_truth.relation_size(); + for (int relation_index = 0; relation_index < ground_truth.relation_size(); ++relation_index) { const Error& error = errors[relation_index]; const proto::Relation& relation = ground_truth.relation(relation_index); @@ -172,13 +173,9 @@ void Run(const std::string& pose_graph_filename, const bool read_text_file_with_unix_timestamps, const bool write_relation_metrics) { LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'..."; - mapping::proto::PoseGraph pose_graph; - { - io::ProtoStreamReader reader(pose_graph_filename); - CHECK(reader.ReadProto(&pose_graph)); - CHECK_EQ(pose_graph.trajectory_size(), 1) - << "Only pose graphs containing a single trajectory are supported."; - } + mapping::proto::PoseGraph pose_graph = + io::DeserializePoseGraphFromFile(pose_graph_filename); + const transform::TransformInterpolationBuffer transform_interpolation_buffer( pose_graph.trajectory(0)); diff --git a/cartographer/io/proto_stream_deserializer.cc b/cartographer/io/proto_stream_deserializer.cc index 6b5eee6..42d6d47 100644 --- a/cartographer/io/proto_stream_deserializer.cc +++ b/cartographer/io/proto_stream_deserializer.cc @@ -17,6 +17,7 @@ #include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/io/internal/mapping_state_serialization.h" +#include "cartographer/io/proto_stream.h" #include "glog/logging.h" namespace cartographer { @@ -35,6 +36,13 @@ bool IsVersionSupported(const mapping::proto::SerializationHeader& header) { } // namespace +mapping::proto::PoseGraph DeserializePoseGraphFromFile( + const std::string& file_name) { + ProtoStreamReader reader(file_name); + ProtoStreamDeserializer deserializer(&reader); + return deserializer.pose_graph(); +} + ProtoStreamDeserializer::ProtoStreamDeserializer( ProtoStreamReaderInterface* const reader) : reader_(reader), header_(ReadHeaderOrDie(reader)) { diff --git a/cartographer/io/proto_stream_deserializer.h b/cartographer/io/proto_stream_deserializer.h index 08424a0..ee0dfd9 100644 --- a/cartographer/io/proto_stream_deserializer.h +++ b/cartographer/io/proto_stream_deserializer.h @@ -25,6 +25,10 @@ namespace cartographer { namespace io { +// Helper function for deserializing the PoseGraph from a proto stream file. +mapping::proto::PoseGraph DeserializePoseGraphFromFile( + const std::string& file_name); + // Helper for deserializing a previously serialized mapping state from a // proto stream, abstracting away the format parsing logic. class ProtoStreamDeserializer { diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 3ee833c..3ad6de5 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -18,6 +18,8 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" +#include "cartographer/io/internal/mapping_state_serialization.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h" #include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h" #include "cartographer/mapping/internal/2d/pose_graph_2d.h" @@ -35,6 +37,23 @@ namespace cartographer { namespace mapping { +namespace { + +using mapping::proto::SerializedData; + +std::vector SelectRangeSensorIds( + const std::set& expected_sensor_ids) { + std::vector range_sensor_ids; + for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) { + if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) { + range_sensor_ids.push_back(sensor_id.id); + } + } + return range_sensor_ids; +} + +} // namespace + proto::MapBuilderOptions CreateMapBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::MapBuilderOptions options; @@ -51,17 +70,6 @@ proto::MapBuilderOptions CreateMapBuilderOptions( return options; } -std::vector SelectRangeSensorIds( - const std::set& expected_sensor_ids) { - std::vector range_sensor_ids; - for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) { - if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) { - range_sensor_ids.push_back(sensor_id.id); - } - } - return range_sensor_ids; -} - MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) : options_(options), thread_pool_(options.num_background_threads()) { CHECK(options.use_trajectory_builder_2d() ^ @@ -176,8 +184,7 @@ void MapBuilder::FinishTrajectory(const int trajectory_id) { } std::string MapBuilder::SubmapToProto( - const mapping::SubmapId& submap_id, - proto::SubmapQuery::Response* const response) { + const SubmapId& submap_id, proto::SubmapQuery::Response* const response) { if (submap_id.trajectory_id < 0 || submap_id.trajectory_id >= num_trajectory_builders()) { return "Requested submap from trajectory " + @@ -196,146 +203,18 @@ std::string MapBuilder::SubmapToProto( } void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) { - // We serialize the pose graph followed by all the data referenced in it. - writer->WriteProto(pose_graph_->ToProto()); - // Serialize trajectory builder options. - { - proto::AllTrajectoryBuilderOptions all_builder_options_proto; - for (const auto& options_with_sensor_ids : - all_trajectory_builder_options_) { - *all_builder_options_proto.add_options_with_sensor_ids() = - options_with_sensor_ids; - } - CHECK_EQ(all_trajectory_builder_options_.size(), - all_builder_options_proto.options_with_sensor_ids_size()); - writer->WriteProto(all_builder_options_proto); - } - // Next we serialize all submap data. - { - for (const auto& submap_id_data : pose_graph_->GetAllSubmapData()) { - proto::LegacySerializedData proto; - auto* const submap_proto = proto.mutable_submap(); - submap_proto->mutable_submap_id()->set_trajectory_id( - submap_id_data.id.trajectory_id); - submap_proto->mutable_submap_id()->set_submap_index( - submap_id_data.id.submap_index); - submap_id_data.data.submap->ToProto( - submap_proto, true /* include_probability_grid_data */); - writer->WriteProto(proto); - } - } - // Next we serialize all node data. - { - for (const auto& node_id_data : pose_graph_->GetTrajectoryNodes()) { - proto::LegacySerializedData proto; - auto* const node_proto = proto.mutable_node(); - node_proto->mutable_node_id()->set_trajectory_id( - node_id_data.id.trajectory_id); - node_proto->mutable_node_id()->set_node_index(node_id_data.id.node_index); - *node_proto->mutable_node_data() = - ToProto(*node_id_data.data.constant_data); - writer->WriteProto(proto); - } - } - // Next we serialize IMU data from the pose graph. - { - const auto all_imu_data = pose_graph_->GetImuData(); - for (const int trajectory_id : all_imu_data.trajectory_ids()) { - for (const auto& imu_data : all_imu_data.trajectory(trajectory_id)) { - proto::LegacySerializedData proto; - auto* const imu_data_proto = proto.mutable_imu_data(); - imu_data_proto->set_trajectory_id(trajectory_id); - *imu_data_proto->mutable_imu_data() = sensor::ToProto(imu_data); - writer->WriteProto(proto); - } - } - } - // Next we serialize odometry data from the pose graph. - { - const auto all_odometry_data = pose_graph_->GetOdometryData(); - for (const int trajectory_id : all_odometry_data.trajectory_ids()) { - for (const auto& odometry_data : - all_odometry_data.trajectory(trajectory_id)) { - proto::LegacySerializedData proto; - auto* const odometry_data_proto = proto.mutable_odometry_data(); - odometry_data_proto->set_trajectory_id(trajectory_id); - *odometry_data_proto->mutable_odometry_data() = - sensor::ToProto(odometry_data); - writer->WriteProto(proto); - } - } - } - // Next we serialize all fixed frame pose data from the pose graph. - { - const auto all_fixed_frame_pose_data = pose_graph_->GetFixedFramePoseData(); - for (const int trajectory_id : all_fixed_frame_pose_data.trajectory_ids()) { - for (const auto& fixed_frame_pose_data : - all_fixed_frame_pose_data.trajectory(trajectory_id)) { - proto::LegacySerializedData proto; - auto* const fixed_frame_pose_data_proto = - proto.mutable_fixed_frame_pose_data(); - fixed_frame_pose_data_proto->set_trajectory_id(trajectory_id); - *fixed_frame_pose_data_proto->mutable_fixed_frame_pose_data() = - sensor::ToProto(fixed_frame_pose_data); - writer->WriteProto(proto); - } - } - } - // Next we serialize all trajectory data. - { - const auto all_trajectory_data = pose_graph_->GetTrajectoryData(); - for (const auto& trajectory_data : all_trajectory_data) { - proto::LegacySerializedData proto; - auto* const trajectory_data_proto = proto.mutable_trajectory_data(); - trajectory_data_proto->set_trajectory_id(trajectory_data.first); - trajectory_data_proto->set_gravity_constant( - trajectory_data.second.gravity_constant); - *trajectory_data_proto->mutable_imu_calibration() = transform::ToProto( - Eigen::Quaterniond(trajectory_data.second.imu_calibration[0], - trajectory_data.second.imu_calibration[1], - trajectory_data.second.imu_calibration[2], - trajectory_data.second.imu_calibration[3])); - if (trajectory_data.second.fixed_frame_origin_in_map.has_value()) { - *trajectory_data_proto->mutable_fixed_frame_origin_in_map() = - transform::ToProto( - trajectory_data.second.fixed_frame_origin_in_map.value()); - } - writer->WriteProto(proto); - } - } - // Next we serialize all landmark data. - { - const std::map - all_landmark_nodes = pose_graph_->GetLandmarkNodes(); - for (const auto& node : all_landmark_nodes) { - for (const auto& observation : node.second.landmark_observations) { - proto::LegacySerializedData proto; - auto* landmark_data_proto = proto.mutable_landmark_data(); - landmark_data_proto->set_trajectory_id(observation.trajectory_id); - landmark_data_proto->mutable_landmark_data()->set_timestamp( - common::ToUniversal(observation.time)); - auto* observation_proto = landmark_data_proto->mutable_landmark_data() - ->add_landmark_observations(); - observation_proto->set_id(node.first); - *observation_proto->mutable_landmark_to_tracking_transform() = - transform::ToProto(observation.landmark_to_tracking_transform); - observation_proto->set_translation_weight( - observation.translation_weight); - observation_proto->set_rotation_weight(observation.rotation_weight); - writer->WriteProto(proto); - } - } - } + io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer); } void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader, bool load_frozen_state) { - proto::PoseGraph pose_graph_proto; - CHECK(reader->ReadProto(&pose_graph_proto)); - proto::AllTrajectoryBuilderOptions all_builder_options_proto; - CHECK(reader->ReadProto(&all_builder_options_proto)); - CHECK_EQ(pose_graph_proto.trajectory_size(), - all_builder_options_proto.options_with_sensor_ids_size()); + io::ProtoStreamDeserializer deserializer(reader); + + // Create a copy of the pose_graph_proto, such that we can re-write the + // trajectory ids. + proto::PoseGraph pose_graph_proto = deserializer.pose_graph(); + const auto& all_builder_options_proto = + deserializer.all_trajectory_builder_options(); std::map trajectory_remapping; for (auto& trajectory_proto : *pose_graph_proto.mutable_trajectory()) { @@ -389,55 +268,76 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader, transform::ToRigid3(landmark.global_pose())); } - for (;;) { - proto::LegacySerializedData proto; - if (!reader->ReadProto(&proto)) { - break; - } - if (proto.has_node()) { - proto.mutable_node()->mutable_node_id()->set_trajectory_id( - trajectory_remapping.at(proto.node().node_id().trajectory_id())); - const transform::Rigid3d node_pose = - node_poses.at(NodeId{proto.node().node_id().trajectory_id(), - proto.node().node_id().node_index()}); - pose_graph_->AddNodeFromProto(node_pose, proto.node()); - } - if (proto.has_submap()) { - proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( - trajectory_remapping.at(proto.submap().submap_id().trajectory_id())); - const transform::Rigid3d submap_pose = - submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(), - proto.submap().submap_id().submap_index()}); - pose_graph_->AddSubmapFromProto(submap_pose, proto.submap()); - } - if (proto.has_trajectory_data()) { - proto.mutable_trajectory_data()->set_trajectory_id( - trajectory_remapping.at(proto.trajectory_data().trajectory_id())); - pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data()); - } - if (!load_frozen_state) { - if (proto.has_imu_data()) { + SerializedData proto; + while (deserializer.ReadNextSerializedData(&proto)) { + switch (proto.data_case()) { + case SerializedData::kPoseGraph: + LOG(ERROR) << "Found multiple serialized `PoseGraph`. Serialized " + "stream likely corrupt!."; + break; + case SerializedData::kAllTrajectoryBuilderOptions: + LOG(ERROR) << "Found multiple serialized " + "`AllTrajectoryBuilderOptions`. Serialized stream likely " + "corrupt!."; + break; + case SerializedData::kSubmap: { + proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( + trajectory_remapping.at( + proto.submap().submap_id().trajectory_id())); + const transform::Rigid3d& submap_pose = submap_poses.at( + SubmapId{proto.submap().submap_id().trajectory_id(), + proto.submap().submap_id().submap_index()}); + pose_graph_->AddSubmapFromProto(submap_pose, proto.submap()); + break; + } + case SerializedData::kNode: { + proto.mutable_node()->mutable_node_id()->set_trajectory_id( + trajectory_remapping.at(proto.node().node_id().trajectory_id())); + const transform::Rigid3d& node_pose = + node_poses.at(NodeId{proto.node().node_id().trajectory_id(), + proto.node().node_id().node_index()}); + pose_graph_->AddNodeFromProto(node_pose, proto.node()); + break; + } + case SerializedData::kTrajectoryData: { + proto.mutable_trajectory_data()->set_trajectory_id( + trajectory_remapping.at(proto.trajectory_data().trajectory_id())); + pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data()); + break; + } + case SerializedData::kImuData: { + if (load_frozen_state) break; pose_graph_->AddImuData( trajectory_remapping.at(proto.imu_data().trajectory_id()), sensor::FromProto(proto.imu_data().imu_data())); + break; } - if (proto.has_odometry_data()) { + case SerializedData::kOdometryData: { + if (load_frozen_state) break; pose_graph_->AddOdometryData( trajectory_remapping.at(proto.odometry_data().trajectory_id()), sensor::FromProto(proto.odometry_data().odometry_data())); + break; } - if (proto.has_fixed_frame_pose_data()) { + case SerializedData::kFixedFramePoseData: { + if (load_frozen_state) break; pose_graph_->AddFixedFramePoseData( trajectory_remapping.at( proto.fixed_frame_pose_data().trajectory_id()), sensor::FromProto( proto.fixed_frame_pose_data().fixed_frame_pose_data())); + break; } - if (proto.has_landmark_data()) { + case SerializedData::kLandmarkData: { + if (load_frozen_state) break; pose_graph_->AddLandmarkData( trajectory_remapping.at(proto.landmark_data().trajectory_id()), sensor::FromProto(proto.landmark_data().landmark_data())); + break; } + default: + LOG(WARNING) << "Skipping unknown message type in stream: " + << proto.GetTypeName(); } } @@ -447,7 +347,7 @@ void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader, for (const proto::PoseGraph::Constraint& constraint_proto : pose_graph_proto.constraint()) { if (constraint_proto.tag() != - mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) { + proto::PoseGraph::Constraint::INTRA_SUBMAP) { continue; } pose_graph_->AddNodeToSubmap(