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.
master
Sebastian Klose 2018-05-30 13:31:33 +02:00 committed by Wally B. Feed
parent 87370371ec
commit 73d18e5fc5
8 changed files with 118 additions and 214 deletions

View File

@ -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<handlers::LoadStateSignature> 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));
}

View File

@ -36,11 +36,9 @@ void WriteStateHandler::OnRequest(const google::protobuf::Empty& request) {
}
auto response = common::make_unique<proto::WriteStateResponse>();
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);

View File

@ -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)

View File

@ -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,

View File

@ -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<Error>& 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));

View File

@ -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)) {

View File

@ -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 {

View File

@ -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<std::string> SelectRangeSensorIds(
const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
std::vector<std::string> 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<std::string> SelectRangeSensorIds(
const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
std::vector<std::string> 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<std::string /* landmark ID */, PoseGraph::LandmarkNode>
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<int, int> 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(