From c1fbb6bb07e25f0a205371ac5feaa395a8a20a43 Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Tue, 31 Jul 2018 11:48:04 +0200
Subject: [PATCH] Fill submaps slices (#1353)

This implements a frequently used function (two times in cartographer_ros).
Also, it corrects a wrong repeated proto field access,
so we can fix cartographer_ros/issues/944.
---
 cartographer/io/submap_painter.cc | 35 +++++++++++++++++++++++++++++++
 cartographer/io/submap_painter.h  |  6 ++++++
 2 files changed, 41 insertions(+)

diff --git a/cartographer/io/submap_painter.cc b/cartographer/io/submap_painter.cc
index dbaa6df..49b0046 100644
--- a/cartographer/io/submap_painter.cc
+++ b/cartographer/io/submap_painter.cc
@@ -57,6 +57,16 @@ void CairoPaintSubmapSlices(
   }
 }
 
+bool Has2DGrid(const mapping::proto::Submap& submap) {
+  return submap.has_submap_2d() && submap.submap_2d().has_grid();
+}
+
+bool Has3DGrids(const mapping::proto::Submap& submap) {
+  return submap.has_submap_3d() &&
+         submap.submap_3d().has_low_resolution_hybrid_grid() &&
+         submap.submap_3d().has_high_resolution_hybrid_grid();
+}
+
 }  // namespace
 
 PaintSubmapSlicesResult PaintSubmapSlices(
@@ -139,6 +149,31 @@ void FillSubmapSlice(
                   texture_proto.height(), &submap_slice->cairo_data);
 }
 
+void DeserializeAndFillSubmapSlices(
+    ProtoStreamDeserializer* deserializer,
+    std::map<mapping::SubmapId, SubmapSlice>* submap_slices,
+    mapping::ValueConversionTables* conversion_tables) {
+  std::map<mapping::SubmapId, transform::Rigid3d> submap_poses;
+  for (const auto& trajectory : deserializer->pose_graph().trajectory()) {
+    for (const auto& submap : trajectory.submap()) {
+      submap_poses[mapping::SubmapId(trajectory.trajectory_id(),
+                                     submap.submap_index())] =
+          transform::ToRigid3(submap.pose());
+    }
+  }
+  mapping::proto::SerializedData proto;
+  while (deserializer->ReadNextSerializedData(&proto)) {
+    if (proto.has_submap() &&
+        (Has2DGrid(proto.submap()) || Has3DGrids(proto.submap()))) {
+      const auto& submap = proto.submap();
+      const mapping::SubmapId id{submap.submap_id().trajectory_id(),
+                                 submap.submap_id().submap_index()};
+      FillSubmapSlice(submap_poses.at(id), submap, &(*submap_slices)[id],
+                      conversion_tables);
+    }
+  }
+}
+
 SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells,
                                         const int width, const int height) {
   SubmapTexture::Pixels pixels;
diff --git a/cartographer/io/submap_painter.h b/cartographer/io/submap_painter.h
index c322b73..f793494 100644
--- a/cartographer/io/submap_painter.h
+++ b/cartographer/io/submap_painter.h
@@ -20,6 +20,7 @@
 #include "Eigen/Geometry"
 #include "cairo/cairo.h"
 #include "cartographer/io/image.h"
+#include "cartographer/io/proto_stream_deserializer.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/mapping/value_conversion_tables.h"
@@ -84,6 +85,11 @@ void FillSubmapSlice(
     SubmapSlice* const submap_slice,
     mapping::ValueConversionTables* conversion_tables);
 
+void DeserializeAndFillSubmapSlices(
+    ProtoStreamDeserializer* deserializer,
+    std::map<::cartographer::mapping::SubmapId, SubmapSlice>* submap_slices,
+    mapping::ValueConversionTables* conversion_tables);
+
 // Unpacks cell data as provided by the backend into 'intensity' and 'alpha'.
 SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells,
                                         int width, int height);