From 03b9285034373528a497f09c6330e7b947a78898 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Mon, 3 Jul 2017 15:23:15 +0200
Subject: [PATCH] Add support for serializing to a proto stream. (#379)

Also changes the ground truth tools to read from such a file.

This resolves #335. Related to #253.
---
 .../ground_truth/autogenerate_ground_truth_main.cc        | 8 +++++---
 .../ground_truth/compute_relations_metrics_main.cc        | 7 ++++---
 cartographer/mapping/map_builder.cc                       | 8 ++++++++
 cartographer/mapping/map_builder.h                        | 4 ++++
 4 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/cartographer/ground_truth/autogenerate_ground_truth_main.cc b/cartographer/ground_truth/autogenerate_ground_truth_main.cc
index 3031d01..c14a379 100644
--- a/cartographer/ground_truth/autogenerate_ground_truth_main.cc
+++ b/cartographer/ground_truth/autogenerate_ground_truth_main.cc
@@ -20,6 +20,7 @@
 
 #include "cartographer/common/port.h"
 #include "cartographer/ground_truth/proto/relations.pb.h"
+#include "cartographer/io/proto_stream.h"
 #include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
 #include "cartographer/transform/transform.h"
 #include "gflags/gflags.h"
@@ -27,7 +28,8 @@
 
 DEFINE_string(
     pose_graph_filename, "",
-    "File with the pose graph proto from which to generate ground truth data.");
+    "Proto stream file containing the pose graph used to generate ground truth "
+    "data.");
 DEFINE_string(output_filename, "", "File to write the ground truth proto to.");
 DEFINE_double(min_covered_distance, 100.,
               "Minimum covered distance in meters before a loop closure is "
@@ -168,8 +170,8 @@ void Run(const string& pose_graph_filename, const string& output_filename,
   LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
   mapping::proto::SparsePoseGraph pose_graph;
   {
-    std::ifstream stream(pose_graph_filename.c_str());
-    CHECK(pose_graph.ParseFromIstream(&stream));
+    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.";
   }
diff --git a/cartographer/ground_truth/compute_relations_metrics_main.cc b/cartographer/ground_truth/compute_relations_metrics_main.cc
index 2bebfbb..a446c01 100644
--- a/cartographer/ground_truth/compute_relations_metrics_main.cc
+++ b/cartographer/ground_truth/compute_relations_metrics_main.cc
@@ -27,6 +27,7 @@
 #include "cartographer/common/port.h"
 #include "cartographer/ground_truth/proto/relations.pb.h"
 #include "cartographer/ground_truth/relations_text_file.h"
+#include "cartographer/io/proto_stream.h"
 #include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
@@ -36,7 +37,7 @@
 
 DEFINE_string(
     pose_graph_filename, "",
-    "File with the pose graph proto from which to assess the quality.");
+    "Proto stream file containing the pose graph used to assess quality.");
 DEFINE_string(relations_filename, "",
               "Relations file containing the ground truth.");
 DEFINE_bool(read_text_file_with_unix_timestamps, false,
@@ -113,8 +114,8 @@ void Run(const string& pose_graph_filename, const string& relations_filename,
   LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
   mapping::proto::SparsePoseGraph pose_graph;
   {
-    std::ifstream stream(pose_graph_filename.c_str());
-    CHECK(pose_graph.ParseFromIstream(&stream));
+    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.";
   }
diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc
index 3a2e2e9..1ff497c 100644
--- a/cartographer/mapping/map_builder.cc
+++ b/cartographer/mapping/map_builder.cc
@@ -132,6 +132,14 @@ string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id,
   return "";
 }
 
+void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
+  // We serialize the pose graph followed by all the data referenced in it.
+  writer->WriteProto(sparse_pose_graph_->ToProto());
+  // TODO(whess): Serialize submaps.
+  // TODO(whess): Serialize range data ("scan") for each trajectory node.
+  // TODO(whess): Serialize additional sensor data: IMU, odometry.
+}
+
 int MapBuilder::num_trajectory_builders() const {
   return trajectory_builders_.size();
 }
diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h
index 7806d65..ac94d2f 100644
--- a/cartographer/mapping/map_builder.h
+++ b/cartographer/mapping/map_builder.h
@@ -27,6 +27,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/thread_pool.h"
+#include "cartographer/io/proto_stream.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/mapping/proto/map_builder_options.pb.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
@@ -77,6 +78,9 @@ class MapBuilder {
   string SubmapToProto(const SubmapId& submap_id,
                        proto::SubmapQuery::Response* response);
 
+  // Serializes the current state to a proto stream.
+  void SerializeState(io::ProtoStreamWriter* writer);
+
   int num_trajectory_builders() const;
 
   mapping::SparsePoseGraph* sparse_pose_graph();