From 31b5a6f1a996044c3fb9ed23be68b4d6a06f9ba7 Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 13 Sep 2017 15:47:02 +0200 Subject: [PATCH] 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. --- .../mapping/proto/submap_visualization.proto | 28 +++++---- cartographer/mapping_2d/submaps.cc | 12 ++-- cartographer/mapping_3d/submaps.cc | 63 +++++++++++-------- 3 files changed, 60 insertions(+), 43 deletions(-) diff --git a/cartographer/mapping/proto/submap_visualization.proto b/cartographer/mapping/proto/submap_visualization.proto index e3742a1..e8c0cdc 100644 --- a/cartographer/mapping/proto/submap_visualization.proto +++ b/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 - // consists of two bytes: value (premultiplied by alpha) and alpha. - optional bytes cells = 3; + // Texture that visualizes a grid of a submap. + message SubmapTexture { + // 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. - optional int32 width = 4; - optional int32 height = 5; + // Dimensions of the grid in cells. + optional int32 width = 2; + optional int32 height = 3; - // Size of one cell in meters. - optional double resolution = 6; + // Size of one cell in meters. + optional double resolution = 4; - // Pose of the resolution*width x resolution*height rectangle in the submap - // frame. - optional transform.proto.Rigid3d slice_pose = 9; + // Pose of the resolution*width x resolution*height rectangle in the + // submap frame. + 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; diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index 1f78fb9..5dc9af9 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/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); - response->set_height(limits.num_y_cells); + texture->set_width(limits.num_x_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.))); } diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index 720d136..90df263 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/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 voxel_indices_and_probabilities = + ExtractVoxelData(hybrid_grid, global_submap_pose.cast(), + &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 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. - Eigen::Array2i min_index(INT_MAX, INT_MAX); - Eigen::Array2i max_index(INT_MIN, INT_MIN); - const std::vector voxel_indices_and_probabilities = - ExtractVoxelData(high_resolution_hybrid_grid_, - global_submap_pose.cast(), &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 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()))); + AddToTextureProto(high_resolution_hybrid_grid_, global_submap_pose, + response->add_textures()); + AddToTextureProto(low_resolution_hybrid_grid_, global_submap_pose, + response->add_textures()); } void Submap::InsertRangeData(const sensor::RangeData& range_data,