Add OccupancyGridState and SubmapState in cartographer/io (#638)

replaces #599 
related to googlecartographer/cartographer_ros#510

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<void(const SubmapSlice&)> 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

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<uint32_t> 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_