Implements a ProbabilityGridPointsProcessor (#383)

cartographer/io/cairo_types.h

@@ -1,29 +0,0 @@
-#ifndef CARTOGRAPHER_IO_CAIRO_TYPES_H_
-#define CARTOGRAPHER_IO_CAIRO_TYPES_H_
-
-#include <memory>
-
-#include "cairo/cairo.h"
-
-namespace cartographer {
-namespace io {
-namespace cairo {
-
-// std::unique_ptr for Cairo surfaces. The surface is destroyed when the
-// std::unique_ptr is reset or destroyed.
-using UniqueSurfacePtr =
-    std::unique_ptr<cairo_surface_t, void (*)(cairo_surface_t*)>;
-
-// std::unique_ptr for Cairo contexts. The context is destroyed when the
-// std::unique_ptr is reset or destroyed.
-using UniqueContextPtr = std::unique_ptr<cairo_t, void (*)(cairo_t*)>;
-
-// std::unique_ptr for Cairo paths. The path is destroyed when the
-// std::unique_ptr is reset or destroyed.
-using UniquePathPtr = std::unique_ptr<cairo_path_t, void (*)(cairo_path_t*)>;
-
-}  // namespace cairo
-}  // namespace io
-}  // namespace cartographer
-
-#endif  // CARTOGRAPHER_IO_CAIRO_TYPES_H_

cartographer/io/image.cc

@@ -0,0 +1,72 @@
+#include "cartographer/io/image.h"
+
+#include <memory>
+
+#include "cairo/cairo.h"
+#include "cartographer/io/file_writer.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+namespace {
+
+// std::unique_ptr for Cairo surfaces. The surface is destroyed when the
+// std::unique_ptr is reset or destroyed.
+using UniqueSurfacePtr =
+    std::unique_ptr<cairo_surface_t, void (*)(cairo_surface_t*)>;
+
+cairo_status_t CairoWriteCallback(void* const closure,
+                                  const unsigned char* data,
+                                  const unsigned int length) {
+  if (static_cast<FileWriter*>(closure)->Write(
+          reinterpret_cast<const char*>(data), length)) {
+    return CAIRO_STATUS_SUCCESS;
+  }
+  return CAIRO_STATUS_WRITE_ERROR;
+}
+
+constexpr cairo_format_t kCairoFormat = CAIRO_FORMAT_ARGB32;
+
+int StrideForWidth(int width) {
+  const int stride = cairo_format_stride_for_width(kCairoFormat, width);
+  CHECK_EQ(stride % 4, 0);
+  return stride;
+}
+
+}  // namespace
+
+Image::Image(int width, int height)
+    : width_(width),
+      height_(height),
+      stride_(StrideForWidth(width)),
+      pixels_(stride_ / 4 * height, 0) {}
+
+void Image::WritePng(FileWriter* const file_writer) {
+  // TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of
+  // the data until the surface is finalized. Once it is finalized though,
+  // cairo_surface_write_to_png fails, complaining that the surface is already
+  // finalized. This makes it pretty hard to pass back ownership of the image to
+  // the caller.
+  UniqueSurfacePtr surface(cairo_image_surface_create_for_data(
+                               reinterpret_cast<unsigned char*>(pixels_.data()),
+                               kCairoFormat, width_, height_, stride_),
+                           cairo_surface_destroy);
+  CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS);
+  CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback,
+                                             file_writer),
+           CAIRO_STATUS_SUCCESS);
+}
+
+const Color Image::GetPixel(int x, int y) const {
+  const uint32_t value = pixels_[y * stride_ / 4 + x];
+  return {{static_cast<uint8_t>(value >> 16), static_cast<uint8_t>(value >> 8),
+           static_cast<uint8_t>(value)}};
+}
+
+void Image::SetPixel(int x, int y, const Color& color) {
+  pixels_[y * stride_ / 4 + x] =
+      (255 << 24) | (color[0] << 16) | (color[1] << 8) | color[2];
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/image.h

@@ -0,0 +1,31 @@
+#ifndef CARTOGRAPHER_IO_IMAGE_H_
+#define CARTOGRAPHER_IO_IMAGE_H_
+
+#include <cstdint>
+#include <vector>
+
+#include "cartographer/io/file_writer.h"
+#include "cartographer/io/points_batch.h"
+
+namespace cartographer {
+namespace io {
+
+class Image {
+ public:
+  Image(int width, int height);
+
+  const Color GetPixel(int x, int y) const;
+  void SetPixel(int x, int y, const Color& color);
+  void WritePng(FileWriter* const file_writer);
+
+ private:
+  int width_;
+  int height_;
+  int stride_;
+  std::vector<uint32_t> pixels_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_IMAGE_H_

cartographer/io/points_processor_pipeline_builder.cc

@@ -27,6 +27,7 @@
 #include "cartographer/io/outlier_removing_points_processor.h"
 #include "cartographer/io/pcd_writing_points_processor.h"
 #include "cartographer/io/ply_writing_points_processor.h"
+#include "cartographer/io/probability_grid_points_processor.h"
 #include "cartographer/io/xray_points_processor.h"
 #include "cartographer/io/xyz_writing_points_processor.h"
 #include "cartographer/mapping/proto/trajectory.pb.h"
@@ -77,6 +78,8 @@ void RegisterBuiltInPointsProcessors(
       file_writer_factory, builder);
   RegisterFileWritingPointsProcessor<HybridGridPointsProcessor>(
       file_writer_factory, builder);
+  RegisterFileWritingPointsProcessor<ProbabilityGridPointsProcessor>(
+      file_writer_factory, builder);
 
   // X-Ray is an odd ball since it requires the trajectory to figure out the
   // different building levels we walked on to separate the images.

cartographer/io/probability_grid_points_processor.cc

@@ -0,0 +1,108 @@
+#include "cartographer/io/probability_grid_points_processor.h"
+
+#include "Eigen/Core"
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/math.h"
+#include "cartographer/io/image.h"
+#include "cartographer/io/points_batch.h"
+
+namespace cartographer {
+namespace io {
+namespace {
+
+void WriteGrid(const mapping_2d::ProbabilityGrid& probability_grid,
+               FileWriter* const file_writer) {
+  Eigen::Array2i offset;
+  mapping_2d::CellLimits cell_limits;
+  probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
+  if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
+    LOG(WARNING) << "Not writing output: empty probability grid";
+    return;
+  }
+  const auto grid_index_to_pixel = [cell_limits](const Eigen::Array2i& index) {
+    return Eigen::Array2i(cell_limits.num_y_cells - index(1) - 1,
+                          cell_limits.num_x_cells - index(0) - 1);
+  };
+  const auto compute_color_value = [&probability_grid](
+                                       const Eigen::Array2i& index) {
+    if (probability_grid.IsKnown(index)) {
+      const float probability = 1.f - probability_grid.GetProbability(index);
+      return static_cast<uint8_t>(
+          255 * ((probability - mapping::kMinProbability) /
+                 (mapping::kMaxProbability - mapping::kMinProbability)));
+    } else {
+      constexpr uint8_t kUnknownValue = 128;
+      return kUnknownValue;
+    }
+  };
+  int width = cell_limits.num_y_cells;
+  int height = cell_limits.num_x_cells;
+  Image image(width, height);
+  for (auto xy_index :
+       cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) {
+    auto index = xy_index + offset;
+    uint8 value = compute_color_value(index);
+    const Eigen::Array2i pixel = grid_index_to_pixel(xy_index);
+    image.SetPixel(pixel.x(), pixel.y(), {{value, value, value}});
+  }
+  image.WritePng(file_writer);
+  CHECK(file_writer->Close());
+}
+
+mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution) {
+  constexpr int kInitialProbabilityGridSize = 100;
+  Eigen::Vector2d max =
+      0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones();
+  return mapping_2d::ProbabilityGrid(cartographer::mapping_2d::MapLimits(
+      resolution, max,
+      mapping_2d::CellLimits(kInitialProbabilityGridSize,
+                             kInitialProbabilityGridSize)));
+}
+
+}  // namespace
+
+ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
+    const double resolution,
+    const mapping_2d::proto::RangeDataInserterOptions&
+        range_data_inserter_options,
+    std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
+    : next_(next),
+      file_writer_(std::move(file_writer)),
+      range_data_inserter_(range_data_inserter_options),
+      probability_grid_(CreateProbabilityGrid(resolution)) {}
+
+std::unique_ptr<ProbabilityGridPointsProcessor>
+ProbabilityGridPointsProcessor::FromDictionary(
+    FileWriterFactory file_writer_factory,
+    common::LuaParameterDictionary* const dictionary,
+    PointsProcessor* const next) {
+  return common::make_unique<ProbabilityGridPointsProcessor>(
+      dictionary->GetDouble("resolution"),
+      mapping_2d::CreateRangeDataInserterOptions(
+          dictionary->GetDictionary("range_data_inserter").get()),
+      file_writer_factory(dictionary->GetString("filename") + ".png"), next);
+}
+
+void ProbabilityGridPointsProcessor::Process(
+    std::unique_ptr<PointsBatch> batch) {
+  range_data_inserter_.Insert({batch->origin, batch->points, {}},
+                              &probability_grid_);
+  next_->Process(std::move(batch));
+}
+
+PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
+  WriteGrid(probability_grid_, file_writer_.get());
+  switch (next_->Flush()) {
+    case FlushResult::kRestartStream:
+      LOG(FATAL) << "ProbabilityGrid generation must be configured to occur "
+                    "after any stages that require multiple passes.";
+
+    case FlushResult::kFinished:
+      return FlushResult::kFinished;
+  }
+  LOG(FATAL);
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/probability_grid_points_processor.h

@@ -0,0 +1,54 @@
+#ifndef CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_
+
+#include <memory>
+#include <string>
+
+#include "cartographer/io/file_writer.h"
+#include "cartographer/io/points_batch.h"
+#include "cartographer/io/points_processor.h"
+#include "cartographer/mapping_2d/probability_grid.h"
+#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h"
+#include "cartographer/mapping_2d/range_data_inserter.h"
+
+namespace cartographer {
+namespace io {
+
+// Creates a probability grid with the specified 'resolution'. As all points are
+// projected into the x-y plane the z component of the data is ignored.
+// 'range_data_inserter' options are used to configure the range data ray
+// tracing through the probability grid.
+class ProbabilityGridPointsProcessor : public PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName =
+      "write_probability_grid";
+  ProbabilityGridPointsProcessor(
+      double resolution,
+      const mapping_2d::proto::RangeDataInserterOptions&
+          range_data_inserter_options,
+      std::unique_ptr<FileWriter> file_writer, PointsProcessor* next);
+  ProbabilityGridPointsProcessor(const ProbabilityGridPointsProcessor&) =
+      delete;
+  ProbabilityGridPointsProcessor& operator=(
+      const ProbabilityGridPointsProcessor&) = delete;
+
+  static std::unique_ptr<ProbabilityGridPointsProcessor> FromDictionary(
+      FileWriterFactory file_writer_factory,
+      common::LuaParameterDictionary* dictionary, PointsProcessor* next);
+
+  ~ProbabilityGridPointsProcessor() override {}
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  PointsProcessor* const next_;
+  std::unique_ptr<FileWriter> file_writer_;
+  mapping_2d::RangeDataInserter range_data_inserter_;
+  mapping_2d::ProbabilityGrid probability_grid_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_

cartographer/io/xray_points_processor.cc

@@ -20,11 +20,10 @@
 #include <string>
 
 #include "Eigen/Core"
-#include "cairo/cairo.h"
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
-#include "cartographer/io/cairo_types.h"
+#include "cartographer/io/image.h"
 #include "cartographer/mapping/detect_floors.h"
 #include "cartographer/mapping_3d/hybrid_grid.h"
 
@@ -46,22 +45,9 @@ double Mix(const double a, const double b, const double t) {
   return a * (1. - t) + t * b;
 }
 
-cairo_status_t CairoWriteCallback(void* const closure,
-                                  const unsigned char* data,
-                                  const unsigned int length) {
-  if (static_cast<FileWriter*>(closure)->Write(
-          reinterpret_cast<const char*>(data), length)) {
-    return CAIRO_STATUS_SUCCESS;
-  }
-  return CAIRO_STATUS_WRITE_ERROR;
-}
-
 // Write 'mat' as a pleasing-to-look-at PNG into 'filename'
-void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) {
+void WriteImage(const PixelDataMatrix& mat, FileWriter* const file_writer) {
-  const int stride =
+  Image image(mat.cols(), mat.rows());
-      cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols());
-  CHECK_EQ(stride % 4, 0);
-  std::vector<uint32_t> pixels(stride / 4 * mat.rows(), 0.);
 
   float max = std::numeric_limits<float>::min();
   for (int y = 0; y < mat.rows(); ++y) {
@@ -78,8 +64,7 @@ void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) {
     for (int x = 0; x < mat.cols(); ++x) {
       const PixelData& cell = mat(y, x);
       if (cell.num_occupied_cells_in_column == 0.) {
-        pixels[y * stride / 4 + x] =
+        image.SetPixel(x, y, {{255, 255, 255}});
-            (255 << 24) | (255 << 16) | (255 << 8) | 255;
         continue;
       }
 
@@ -96,27 +81,14 @@ void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) {
       double mix_g = Mix(1., mean_g_in_column, saturation);
       double mix_b = Mix(1., mean_b_in_column, saturation);
 
-      const int r = common::RoundToInt(mix_r * 255.);
+      const uint8_t r = common::RoundToInt(mix_r * 255.);
-      const int g = common::RoundToInt(mix_g * 255.);
+      const uint8_t g = common::RoundToInt(mix_g * 255.);
-      const int b = common::RoundToInt(mix_b * 255.);
+      const uint8_t b = common::RoundToInt(mix_b * 255.);
-      pixels[y * stride / 4 + x] = (255 << 24) | (r << 16) | (g << 8) | b;
+      image.SetPixel(x, y, {{r, g, b}});
     }
   }
 
-  // TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of
+  image.WritePng(file_writer);
-  // the data until the surface is finalized. Once it is finalized though,
-  // cairo_surface_write_to_png fails, complaining that the surface is already
-  // finalized. This makes it pretty hard to pass back ownership of the image to
-  // the caller.
-  cairo::UniqueSurfacePtr surface(
-      cairo_image_surface_create_for_data(
-          reinterpret_cast<unsigned char*>(pixels.data()), CAIRO_FORMAT_ARGB32,
-          mat.cols(), mat.rows(), stride),
-      cairo_surface_destroy);
-  CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS);
-  CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback,
-                                             file_writer),
-           CAIRO_STATUS_SUCCESS);
   CHECK(file_writer->Close());
 }
 
@@ -196,7 +168,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
     pixel_data.mean_b = column_data.sum_b / column_data.count;
     ++pixel_data.num_occupied_cells_in_column;
   }
-  WritePng(image, file_writer);
+  WriteImage(image, file_writer);
 }
 
 void XRayPointsProcessor::Insert(const PointsBatch& batch,