From 028391357d4d5fc680bd2eeadf6ff9723973ff57 Mon Sep 17 00:00:00 2001 From: Jihoon Lee Date: Thu, 9 Nov 2017 17:19:29 +0100 Subject: [PATCH] Add OccupancyGridState and SubmapState in cartographer/io (#638) replaces #599 related to googlecartographer/cartographer_ros#510 --- cartographer/io/submap_painter.cc | 112 ++++++++++++++++++++++++++++++ cartographer/io/submap_painter.h | 68 ++++++++++++++++++ 2 files changed, 180 insertions(+) create mode 100644 cartographer/io/submap_painter.cc create mode 100644 cartographer/io/submap_painter.h diff --git a/cartographer/io/submap_painter.cc b/cartographer/io/submap_painter.cc new file mode 100644 index 0000000..32a8653 --- /dev/null +++ b/cartographer/io/submap_painter.cc @@ -0,0 +1,112 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/submap_painter.h" + +namespace cartographer { +namespace io { +namespace { + +Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) { + return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation(); +} + +void CairoPaintSubmapSlices( + const double scale, + std::map<::cartographer::mapping::SubmapId, SubmapSlice>* submaps, + cairo_t* cr, std::function draw_callback) { + cairo_scale(cr, scale, scale); + + for (auto& pair : *submaps) { + auto& submap_slice = pair.second; + if (submap_slice.surface == nullptr) { + return; + } + const Eigen::Matrix4d homo = + ToEigen(submap_slice.pose * submap_slice.slice_pose).matrix(); + + cairo_save(cr); + cairo_matrix_t matrix; + cairo_matrix_init(&matrix, homo(1, 0), homo(0, 0), -homo(1, 1), -homo(0, 1), + homo(0, 3), -homo(1, 3)); + cairo_transform(cr, &matrix); + + const double submap_resolution = submap_slice.resolution; + cairo_scale(cr, submap_resolution, submap_resolution); + + // Invokes caller's callback to utilize slice data in global cooridnate + // frame. e.g. finds bounding box, paints slices. + draw_callback(submap_slice); + cairo_restore(cr); + } +} + +} // namespace + +PaintSubmapSlicesResult PaintSubmapSlices( + std::map<::cartographer::mapping::SubmapId, SubmapSlice>* submaps, + const double resolution) { + Eigen::AlignedBox2f bounding_box; + { + auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( + cairo_image_surface_create(PaintSubmapSlicesResult::kCairoFormat, 1, + 1)); + auto cr = + ::cartographer::io::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)); + }; + + CairoPaintSubmapSlices( + 1. / resolution, submaps, cr.get(), + [&update_bounding_box, &bounding_box](const SubmapSlice& submap_slice) { + update_bounding_box(0, 0); + update_bounding_box(submap_slice.width, 0); + update_bounding_box(0, submap_slice.height); + update_bounding_box(submap_slice.width, submap_slice.height); + }); + } + + const int kPaddingPixel = 5; + const Eigen::Array2i size( + std::ceil(bounding_box.sizes().x()) + 2 * kPaddingPixel, + std::ceil(bounding_box.sizes().y()) + 2 * kPaddingPixel); + const Eigen::Array2f origin(-bounding_box.min().x() + kPaddingPixel, + -bounding_box.min().y() + kPaddingPixel); + + auto surface = + ::cartographer::io::MakeUniqueCairoSurfacePtr(cairo_image_surface_create( + PaintSubmapSlicesResult::kCairoFormat, size.x(), size.y())); + { + auto cr = + ::cartographer::io::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()); + CairoPaintSubmapSlices(1. / resolution, submaps, cr.get(), + [&cr](const SubmapSlice& submap_slice) { + cairo_set_source_surface( + cr.get(), submap_slice.surface.get(), 0., 0.); + cairo_paint(cr.get()); + }); + cairo_surface_flush(surface.get()); + } + return PaintSubmapSlicesResult(std::move(surface), origin); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/submap_painter.h b/cartographer/io/submap_painter.h new file mode 100644 index 0000000..93473d9 --- /dev/null +++ b/cartographer/io/submap_painter.h @@ -0,0 +1,68 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_SUBMAP_PAINTER_H_ +#define CARTOGRAPHER_IO_SUBMAP_PAINTER_H_ + +#include "Eigen/Geometry" +#include "cairo/cairo.h" +#include "cartographer/io/image.h" +#include "cartographer/mapping/id.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace io { + +struct PaintSubmapSlicesResult { + // Data format for 'surface'. + static constexpr cairo_format_t kCairoFormat = CAIRO_FORMAT_ARGB32; + + PaintSubmapSlicesResult(::cartographer::io::UniqueCairoSurfacePtr surface, + Eigen::Array2f origin) + : surface(std::move(surface)), origin(origin) {} + ::cartographer::io::UniqueCairoSurfacePtr surface; + + // Top left pixel of 'surface' in map frame. + Eigen::Array2f origin; +}; + +struct SubmapSlice { + SubmapSlice() + : surface(::cartographer::io::MakeUniqueCairoSurfacePtr(nullptr)) {} + + // Texture data. + int width; + int height; + int version; + double resolution; + ::cartographer::transform::Rigid3d slice_pose; + ::cartographer::io::UniqueCairoSurfacePtr surface; + // Pixel data used by 'surface'. Must outlive 'surface'. + std::vector cairo_data; + + // Metadata. + ::cartographer::transform::Rigid3d pose; + int metadata_version = -1; +}; + +PaintSubmapSlicesResult PaintSubmapSlices( + std::map<::cartographer::mapping::SubmapId, SubmapSlice>* submaps, + const double resolution); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_SUBMAP_PAINTER_H_