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
master
Jihoon Lee 2018-01-04 18:52:05 +09:00 committed by Wally B. Feed
parent 9ee65293d2
commit 2ad83662f2
2 changed files with 119 additions and 9 deletions

View File

@ -61,10 +61,9 @@ PaintSubmapSlicesResult PaintSubmapSlices(
const double resolution) { const double resolution) {
Eigen::AlignedBox2f bounding_box; Eigen::AlignedBox2f bounding_box;
{ {
auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( auto surface = MakeUniqueCairoSurfacePtr(
cairo_image_surface_create(::cartographer::io::kCairoFormat, 1, 1)); cairo_image_surface_create(kCairoFormat, 1, 1));
auto cr = auto cr = MakeUniqueCairoPtr(cairo_create(surface.get()));
::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface.get()));
const auto update_bounding_box = [&bounding_box, &cr](double x, double y) { const auto update_bounding_box = [&bounding_box, &cr](double x, double y) {
cairo_user_to_device(cr.get(), &x, &y); cairo_user_to_device(cr.get(), &x, &y);
bounding_box.extend(Eigen::Vector2f(x, y)); bounding_box.extend(Eigen::Vector2f(x, y));
@ -87,12 +86,10 @@ PaintSubmapSlicesResult PaintSubmapSlices(
const Eigen::Array2f origin(-bounding_box.min().x() + kPaddingPixel, const Eigen::Array2f origin(-bounding_box.min().x() + kPaddingPixel,
-bounding_box.min().y() + kPaddingPixel); -bounding_box.min().y() + kPaddingPixel);
auto surface = auto surface = MakeUniqueCairoSurfacePtr(
::cartographer::io::MakeUniqueCairoSurfacePtr(cairo_image_surface_create( cairo_image_surface_create(kCairoFormat, size.x(), size.y()));
::cartographer::io::kCairoFormat, size.x(), size.y()));
{ {
auto cr = auto cr = MakeUniqueCairoPtr(cairo_create(surface.get()));
::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface.get()));
cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.); cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.);
cairo_paint(cr.get()); cairo_paint(cr.get());
cairo_translate(cr.get(), origin.x(), origin.y()); cairo_translate(cr.get(), origin.x(), origin.y());
@ -107,5 +104,82 @@ PaintSubmapSlicesResult PaintSubmapSlices(
return PaintSubmapSlicesResult(std::move(surface), origin); 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<char>& intensity,
const std::vector<char>& alpha,
const int width, const int height,
std::vector<uint32_t>* 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<unsigned char*>(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 io
} // namespace cartographer } // namespace cartographer

View File

@ -21,6 +21,9 @@
#include "cairo/cairo.h" #include "cairo/cairo.h"
#include "cartographer/io/image.h" #include "cartographer/io/image.h"
#include "cartographer/mapping/id.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" #include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {
@ -55,10 +58,43 @@ struct SubmapSlice {
int metadata_version = -1; int metadata_version = -1;
}; };
struct SubmapTexture {
struct Pixels {
std::vector<char> intensity;
std::vector<char> alpha;
};
Pixels pixels;
int width;
int height;
double resolution;
::cartographer::transform::Rigid3d slice_pose;
};
struct SubmapTextures {
int version;
std::vector<SubmapTexture> textures;
};
PaintSubmapSlicesResult PaintSubmapSlices( PaintSubmapSlicesResult PaintSubmapSlices(
const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps, const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps,
double resolution); 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<char>& intensity,
const std::vector<char>& alpha, int width,
int height,
std::vector<uint32_t>* cairo_data);
} // namespace io } // namespace io
} // namespace cartographer } // namespace cartographer