From 5378ee2adc7596a8b05957c11a3d3e73133d75d6 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Fri, 7 Jul 2017 15:39:55 +0200
Subject: [PATCH] Serialize range data. (#395)

We compress 2D range data before serialization to reduce
output size.

Related to #253.
---
 cartographer/mapping/map_builder.cc           | 63 ++++++++++++++-----
 .../mapping/proto/serialization.proto         | 10 ++-
 2 files changed, 55 insertions(+), 18 deletions(-)

diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc
index 8b46045..7ee5ee3 100644
--- a/cartographer/mapping/map_builder.cc
+++ b/cartographer/mapping/map_builder.cc
@@ -141,25 +141,54 @@ 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());
   // Next we serialize all submap data.
-  const auto submap_data = sparse_pose_graph_->GetAllSubmapData();
-  for (int trajectory_id = 0;
-       trajectory_id != static_cast<int>(submap_data.size()); ++trajectory_id) {
-    for (int submap_index = 0;
-         submap_index != static_cast<int>(submap_data[trajectory_id].size());
-         ++submap_index) {
-      proto::SerializedData proto;
-      auto* const submap_proto = proto.mutable_submap();
-      // TODO(whess): Handle trimmed data.
-      submap_proto->mutable_submap_id()->set_trajectory_id(trajectory_id);
-      submap_proto->mutable_submap_id()->set_submap_index(submap_index);
-      submap_data[trajectory_id][submap_index].submap->ToProto(submap_proto);
-      // TODO(whess): Only enable optionally? Resulting pbstream files will be
-      // a lot larger now.
-      writer->WriteProto(proto);
+  {
+    const auto submap_data = sparse_pose_graph_->GetAllSubmapData();
+    for (int trajectory_id = 0;
+         trajectory_id != static_cast<int>(submap_data.size());
+         ++trajectory_id) {
+      for (int submap_index = 0;
+           submap_index != static_cast<int>(submap_data[trajectory_id].size());
+           ++submap_index) {
+        proto::SerializedData proto;
+        auto* const submap_proto = proto.mutable_submap();
+        // TODO(whess): Handle trimmed data.
+        submap_proto->mutable_submap_id()->set_trajectory_id(trajectory_id);
+        submap_proto->mutable_submap_id()->set_submap_index(submap_index);
+        submap_data[trajectory_id][submap_index].submap->ToProto(submap_proto);
+        // TODO(whess): Only enable optionally? Resulting pbstream files will be
+        // a lot larger now.
+        writer->WriteProto(proto);
+      }
     }
   }
-  // TODO(whess): Serialize range data ("scan") for each trajectory node.
-  // TODO(whess): Serialize additional sensor data: IMU, odometry.
+  // Next we serialize all range data.
+  {
+    const auto node_data = sparse_pose_graph_->GetTrajectoryNodes();
+    for (int trajectory_id = 0;
+         trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
+      for (int node_index = 0;
+           node_index != static_cast<int>(node_data[trajectory_id].size());
+           ++node_index) {
+        proto::SerializedData proto;
+        auto* const range_data_proto = proto.mutable_range_data();
+        // TODO(whess): Handle trimmed data.
+        range_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
+        range_data_proto->mutable_node_id()->set_node_index(node_index);
+        const auto& data = *node_data[trajectory_id][node_index].constant_data;
+        if (!data.range_data_2d.returns.empty()) {
+          *range_data_proto->mutable_range_data_2d() =
+              sensor::ToProto(sensor::Compress(data.range_data_2d));
+        } else {
+          *range_data_proto->mutable_range_data_3d() =
+              sensor::ToProto(data.range_data_3d);
+        }
+        // TODO(whess): Only enable optionally? Resulting pbstream files will be
+        // a lot larger now.
+        writer->WriteProto(proto);
+      }
+    }
+    // TODO(whess): Serialize additional sensor data: IMU, odometry.
+  }
 }
 
 void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
diff --git a/cartographer/mapping/proto/serialization.proto b/cartographer/mapping/proto/serialization.proto
index 480b7ae..d4147d1 100644
--- a/cartographer/mapping/proto/serialization.proto
+++ b/cartographer/mapping/proto/serialization.proto
@@ -18,6 +18,7 @@ package cartographer.mapping.proto;
 
 import "cartographer/mapping/proto/sparse_pose_graph.proto";
 import "cartographer/mapping/proto/submap.proto";
+import "cartographer/sensor/proto/sensor.proto";
 
 message Submap {
   optional SubmapId submap_id = 1;
@@ -25,7 +26,14 @@ message Submap {
   optional Submap3D submap_3d = 3;
 }
 
+message RangeData {
+  optional NodeId node_id = 1;
+  optional sensor.proto.CompressedRangeData range_data_2d = 2;
+  optional sensor.proto.CompressedRangeData range_data_3d = 3;
+}
+
 message SerializedData {
   optional Submap submap = 1;
-  // TODO(whess): Add range data, IMU data, odometry.
+  optional RangeData range_data = 2;
+  // TODO(whess): Add IMU data, odometry.
 }