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,