From 2ad83662f22ccd8f3a664cdf72bc0c2da5c9c874 Mon Sep 17 00:00:00 2001 From: Jihoon Lee Date: Thu, 4 Jan 2018 18:52:05 +0900 Subject: [PATCH] Migrate SubmapTexture and SubmapSlice logics from cartographer_ros (#782) Towards [RFC06](https://github.com/googlecartographer/rfcs/blob/master/text/0006-serve-ros-map-from-pbstream.md). Migrates * `FillSubmapSlice` from `pbstream_to_rosmap_main.cc` * `SubmapTexture` logics from cartographer_ros --- cartographer/io/submap_painter.cc | 92 ++++++++++++++++++++++++++++--- cartographer/io/submap_painter.h | 36 ++++++++++++ 2 files changed, 119 insertions(+), 9 deletions(-) diff --git a/cartographer/io/submap_painter.cc b/cartographer/io/submap_painter.cc index 2498d43..6c446d8 100644 --- a/cartographer/io/submap_painter.cc +++ b/cartographer/io/submap_painter.cc @@ -61,10 +61,9 @@ PaintSubmapSlicesResult PaintSubmapSlices( const double resolution) { Eigen::AlignedBox2f bounding_box; { - auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( - cairo_image_surface_create(::cartographer::io::kCairoFormat, 1, 1)); - auto cr = - ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface.get())); + auto surface = MakeUniqueCairoSurfacePtr( + cairo_image_surface_create(kCairoFormat, 1, 1)); + auto cr = MakeUniqueCairoPtr(cairo_create(surface.get())); const auto update_bounding_box = [&bounding_box, &cr](double x, double y) { cairo_user_to_device(cr.get(), &x, &y); bounding_box.extend(Eigen::Vector2f(x, y)); @@ -87,12 +86,10 @@ PaintSubmapSlicesResult PaintSubmapSlices( const Eigen::Array2f origin(-bounding_box.min().x() + kPaddingPixel, -bounding_box.min().y() + kPaddingPixel); - auto surface = - ::cartographer::io::MakeUniqueCairoSurfacePtr(cairo_image_surface_create( - ::cartographer::io::kCairoFormat, size.x(), size.y())); + auto surface = MakeUniqueCairoSurfacePtr( + cairo_image_surface_create(kCairoFormat, size.x(), size.y())); { - auto cr = - ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface.get())); + auto cr = MakeUniqueCairoPtr(cairo_create(surface.get())); cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.); cairo_paint(cr.get()); cairo_translate(cr.get(), origin.x(), origin.y()); @@ -107,5 +104,82 @@ PaintSubmapSlicesResult PaintSubmapSlices( return PaintSubmapSlicesResult(std::move(surface), origin); } +void FillSubmapSlice( + const ::cartographer::transform::Rigid3d& global_submap_pose, + const ::cartographer::mapping::proto::Submap& proto, + SubmapSlice* const submap_slice) { + ::cartographer::mapping::proto::SubmapQuery::Response response; + ::cartographer::transform::Rigid3d local_pose; + if (proto.has_submap_3d()) { + ::cartographer::mapping_3d::Submap submap(proto.submap_3d()); + local_pose = submap.local_pose(); + submap.ToResponseProto(global_submap_pose, &response); + } else { + ::cartographer::mapping_2d::Submap submap(proto.submap_2d()); + local_pose = submap.local_pose(); + submap.ToResponseProto(global_submap_pose, &response); + } + submap_slice->pose = global_submap_pose; + + auto& texture_proto = response.textures(0); + const SubmapTexture::Pixels pixels = UnpackTextureData( + texture_proto.cells(), texture_proto.width(), texture_proto.height()); + submap_slice->width = texture_proto.width(); + submap_slice->height = texture_proto.height(); + submap_slice->resolution = texture_proto.resolution(); + submap_slice->slice_pose = + ::cartographer::transform::ToRigid3(texture_proto.slice_pose()); + submap_slice->surface = + DrawTexture(pixels.intensity, pixels.alpha, texture_proto.width(), + texture_proto.height(), &submap_slice->cairo_data); +} + +SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells, + const int width, const int height) { + SubmapTexture::Pixels pixels; + std::string cells; + ::cartographer::common::FastGunzipString(compressed_cells, &cells); + const int num_pixels = width * height; + CHECK_EQ(cells.size(), 2 * num_pixels); + pixels.intensity.reserve(num_pixels); + pixels.alpha.reserve(num_pixels); + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + pixels.intensity.push_back(cells[(i * width + j) * 2]); + pixels.alpha.push_back(cells[(i * width + j) * 2 + 1]); + } + } + return pixels; +} + +UniqueCairoSurfacePtr DrawTexture(const std::vector& intensity, + const std::vector& alpha, + const int width, const int height, + std::vector* const cairo_data) { + CHECK(cairo_data->empty()); + + // Properly dealing with a non-common stride would make this code much more + // complicated. Let's check that it is not needed. + const int expected_stride = 4 * width; + CHECK_EQ(expected_stride, cairo_format_stride_for_width(kCairoFormat, width)); + for (size_t i = 0; i < intensity.size(); ++i) { + // We use the red channel to track intensity information. The green + // channel we use to track if a cell was ever observed. + const uint8_t intensity_value = intensity.at(i); + const uint8_t alpha_value = alpha.at(i); + const uint8_t observed = + (intensity_value == 0 && alpha_value == 0) ? 0 : 255; + cairo_data->push_back((alpha_value << 24) | (intensity_value << 16) | + (observed << 8) | 0); + } + + auto surface = MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data( + reinterpret_cast(cairo_data->data()), kCairoFormat, width, + height, expected_stride)); + CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS) + << cairo_status_to_string(cairo_surface_status(surface.get())); + return surface; +} + } // namespace io } // namespace cartographer diff --git a/cartographer/io/submap_painter.h b/cartographer/io/submap_painter.h index 8a20389..f9d5d36 100644 --- a/cartographer/io/submap_painter.h +++ b/cartographer/io/submap_painter.h @@ -21,6 +21,9 @@ #include "cairo/cairo.h" #include "cartographer/io/image.h" #include "cartographer/mapping/id.h" +#include "cartographer/mapping/proto/submap.pb.h" +#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_3d/submaps.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { @@ -55,10 +58,43 @@ struct SubmapSlice { int metadata_version = -1; }; +struct SubmapTexture { + struct Pixels { + std::vector intensity; + std::vector alpha; + }; + Pixels pixels; + int width; + int height; + double resolution; + ::cartographer::transform::Rigid3d slice_pose; +}; + +struct SubmapTextures { + int version; + std::vector textures; +}; + PaintSubmapSlicesResult PaintSubmapSlices( const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps, double resolution); +void FillSubmapSlice( + const ::cartographer::transform::Rigid3d& global_submap_pose, + const ::cartographer::mapping::proto::Submap& proto, + SubmapSlice* const submap_slice); + +// Unpacks cell data as provided by the backend into 'intensity' and 'alpha'. +SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells, + int width, int height); + +// Draw a texture into a cairo surface. 'cairo_data' will store the pixel data +// for the surface and must therefore outlive the use of the surface. +UniqueCairoSurfacePtr DrawTexture(const std::vector& intensity, + const std::vector& alpha, int width, + int height, + std::vector* cairo_data); + } // namespace io } // namespace cartographer