Allow multiple SubmapTextures. (#519)

This changes submap_visualization.proto that multiple textures can be provided.
As of now, the first texture is generated from the high resolution
grid, which is the same as before.
The second texture is generated from the low resolution grid.

cartographer/mapping/proto/submap_visualization.proto

@@ -43,20 +43,26 @@ message SubmapQuery {
     // Version of the given submap, higher means newer.
     optional int32 submap_version = 2;
 
-    // GZipped map data, in row-major order, starting with (0,0). Each cell
+    // Texture that visualizes a grid of a submap.
-    // consists of two bytes: value (premultiplied by alpha) and alpha.
+    message SubmapTexture {
-    optional bytes cells = 3;
+      // GZipped map data, in row-major order, starting with (0,0). Each cell
+      // consists of two bytes: value (premultiplied by alpha) and alpha.
+      optional bytes cells = 1;
 
-    // Dimensions of the grid in cells.
+      // Dimensions of the grid in cells.
-    optional int32 width = 4;
+      optional int32 width = 2;
-    optional int32 height = 5;
+      optional int32 height = 3;
 
-    // Size of one cell in meters.
+      // Size of one cell in meters.
-    optional double resolution = 6;
+      optional double resolution = 4;
 
-    // Pose of the resolution*width x resolution*height rectangle in the submap
+      // Pose of the resolution*width x resolution*height rectangle in the
-    // frame.
+      // submap frame.
-    optional transform.proto.Rigid3d slice_pose = 9;
+      optional transform.proto.Rigid3d slice_pose = 5;
+    }
+
+    // When multiple textures are present, high resolution comes first.
+    repeated SubmapTexture textures = 10;
 
     // Error message in response to malformed requests.
     optional string error_message = 8;

cartographer/mapping_2d/submaps.cc

@@ -113,17 +113,19 @@ void Submap::ToResponseProto(
       cells.push_back(0);                                    // alpha
     }
   }
-  common::FastGzipString(cells, response->mutable_cells());
+  mapping::proto::SubmapQuery::Response::SubmapTexture* const texture =
+      response->add_textures();
+  common::FastGzipString(cells, texture->mutable_cells());
 
-  response->set_width(limits.num_x_cells);
+  texture->set_width(limits.num_x_cells);
-  response->set_height(limits.num_y_cells);
+  texture->set_height(limits.num_y_cells);
   const double resolution = probability_grid_.limits().resolution();
-  response->set_resolution(resolution);
+  texture->set_resolution(resolution);
   const double max_x =
       probability_grid_.limits().max().x() - resolution * offset.y();
   const double max_y =
       probability_grid_.limits().max().y() - resolution * offset.x();
-  *response->mutable_slice_pose() = transform::ToProto(
+  *texture->mutable_slice_pose() = transform::ToProto(
       local_pose().inverse() *
       transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
 }

cartographer/mapping_3d/submaps.cc

@@ -146,6 +146,38 @@ string ComputePixelValues(
   return cell_data;
 }
 
+void AddToTextureProto(
+    const HybridGrid& hybrid_grid, const transform::Rigid3d& global_submap_pose,
+    mapping::proto::SubmapQuery::Response::SubmapTexture* const texture) {
+  // Generate an X-ray view through the 'hybrid_grid', aligned to the
+  // xy-plane in the global map frame.
+  const float resolution = hybrid_grid.resolution();
+  texture->set_resolution(resolution);
+
+  // Compute a bounding box for the texture.
+  Eigen::Array2i min_index(INT_MAX, INT_MAX);
+  Eigen::Array2i max_index(INT_MIN, INT_MIN);
+  const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
+      ExtractVoxelData(hybrid_grid, global_submap_pose.cast<float>(),
+                       &min_index, &max_index);
+
+  const int width = max_index.y() - min_index.y() + 1;
+  const int height = max_index.x() - min_index.x() + 1;
+  texture->set_width(width);
+  texture->set_height(height);
+
+  const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
+      width, height, min_index, max_index, voxel_indices_and_probabilities);
+  const string cell_data = ComputePixelValues(accumulated_pixel_data);
+
+  common::FastGzipString(cell_data, texture->mutable_cells());
+  *texture->mutable_slice_pose() = transform::ToProto(
+      global_submap_pose.inverse() *
+      transform::Rigid3d::Translation(Eigen::Vector3d(
+          max_index.x() * resolution, max_index.y() * resolution,
+          global_submap_pose.translation().z())));
+}
+
 }  // namespace
 
 proto::SubmapsOptions CreateSubmapsOptions(
@@ -194,34 +226,11 @@ void Submap::ToResponseProto(
     const transform::Rigid3d& global_submap_pose,
     mapping::proto::SubmapQuery::Response* const response) const {
   response->set_submap_version(num_range_data());
-  // Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
-  // in the global map frame.
-  const float resolution = high_resolution_hybrid_grid_.resolution();
-  response->set_resolution(resolution);
 
-  // Compute a bounding box for the texture.
+  AddToTextureProto(high_resolution_hybrid_grid_, global_submap_pose,
-  Eigen::Array2i min_index(INT_MAX, INT_MAX);
+                    response->add_textures());
-  Eigen::Array2i max_index(INT_MIN, INT_MIN);
+  AddToTextureProto(low_resolution_hybrid_grid_, global_submap_pose,
-  const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
+                    response->add_textures());
-      ExtractVoxelData(high_resolution_hybrid_grid_,
-                       global_submap_pose.cast<float>(), &min_index,
-                       &max_index);
-
-  const int width = max_index.y() - min_index.y() + 1;
-  const int height = max_index.x() - min_index.x() + 1;
-  response->set_width(width);
-  response->set_height(height);
-
-  const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
-      width, height, min_index, max_index, voxel_indices_and_probabilities);
-  const string cell_data = ComputePixelValues(accumulated_pixel_data);
-
-  common::FastGzipString(cell_data, response->mutable_cells());
-  *response->mutable_slice_pose() = transform::ToProto(
-      global_submap_pose.inverse() *
-      transform::Rigid3d::Translation(Eigen::Vector3d(
-          max_index.x() * resolution, max_index.y() * resolution,
-          global_submap_pose.translation().z())));
 }
 
 void Submap::InsertRangeData(const sensor::RangeData& range_data,