Pull some functionality from cartographer_ros into cartographer. (#420)

- Moved color.h.
- Moved Cairo types.
- Simplifications in XRayPointsProcessor.

cartographer/io/color.cc

@@ -0,0 +1,77 @@
+/*
+ * 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/color.h"
+
+#include <cmath>
+
+#include "cartographer/common/port.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+
+namespace {
+
+constexpr float kInitialHue = 0.69f;
+constexpr float kSaturation = 0.85f;
+constexpr float kValue = 0.77f;
+constexpr float kGoldenRatioConjugate = (std::sqrt(5.f) - 1.f) / 2.f;
+
+Color CreateRgba(const float r, const float g, const float b) {
+  return Color{{static_cast<uint8_t>(common::RoundToInt(r * 255.)),
+                static_cast<uint8_t>(common::RoundToInt(g * 255.)),
+                static_cast<uint8_t>(common::RoundToInt(b * 255.))}};
+}
+
+Color HsvToRgb(const float h, const float s, const float v) {
+  const float h_6 = (h == 1.f) ? 0.f : 6 * h;
+  const int h_i = std::floor(h_6);
+  const float f = h_6 - h_i;
+
+  const float p = v * (1.f - s);
+  const float q = v * (1.f - f * s);
+  const float t = v * (1.f - (1.f - f) * s);
+
+  if (h_i == 0) {
+    return CreateRgba(v, t, p);
+  } else if (h_i == 1) {
+    return CreateRgba(q, v, p);
+  } else if (h_i == 2) {
+    return CreateRgba(p, v, t);
+  } else if (h_i == 3) {
+    return CreateRgba(p, q, v);
+  } else if (h_i == 4) {
+    return CreateRgba(t, p, v);
+  } else if (h_i == 5) {
+    return CreateRgba(v, p, q);
+  } else {
+    return CreateRgba(0.f, 0.f, 0.f);
+  }
+}
+
+}  // namespace
+
+Color GetColor(int id) {
+  CHECK_GE(id, 0);
+  // Uniform color sampling using the golden ratio from
+  // http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/
+  const float hue = std::fmod(kInitialHue + kGoldenRatioConjugate * id, 1.f);
+  return HsvToRgb(hue, kSaturation, kValue);
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/color.h

@@ -0,0 +1,35 @@
+/*
+ * 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_COLOR_H_
+#define CARTOGRAPHER_IO_COLOR_H_
+
+#include <array>
+
+namespace cartographer {
+namespace io {
+
+// TODO(hrapp): Should probably be represented as floats.
+using Color = std::array<uint8_t, 3>;
+
+// A function for on-demand generation of a color palette, with every two
+// direct successors having large contrast.
+Color GetColor(int id);
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_COLOR_H_

cartographer/io/image.cc

@@ -2,7 +2,6 @@
 
 #include <memory>
 
-#include "cairo/cairo.h"
 #include "cartographer/io/file_writer.h"
 #include "glog/logging.h"
 
@@ -10,11 +9,6 @@ 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) {
@@ -35,6 +29,14 @@ int StrideForWidth(int width) {
 
 }  // namespace
 
+UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface) {
+  return UniqueCairoSurfacePtr(surface, cairo_surface_destroy);
+}
+
+UniqueCairoPtr MakeUniqueCairoPtr(cairo_t* surface) {
+  return UniqueCairoPtr(surface, cairo_destroy);
+}
+
 Image::Image(int width, int height)
     : width_(width),
       height_(height),
@@ -47,10 +49,7 @@ void Image::WritePng(FileWriter* const file_writer) {
   // 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(
+  UniqueCairoSurfacePtr surface = GetCairoSurface();
-                               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),
@@ -68,5 +67,11 @@ void Image::SetPixel(int x, int y, const Color& color) {
       (255 << 24) | (color[0] << 16) | (color[1] << 8) | color[2];
 }
 
+UniqueCairoSurfacePtr Image::GetCairoSurface() {
+  return MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data(
+      reinterpret_cast<unsigned char*>(pixels_.data()), kCairoFormat, width_,
+      height_, stride_));
+}
+
 }  // namespace io
 }  // namespace cartographer

cartographer/io/image.h

@@ -4,12 +4,27 @@
 #include <cstdint>
 #include <vector>
 
+#include "cairo/cairo.h"
 #include "cartographer/io/file_writer.h"
 #include "cartographer/io/points_batch.h"
 
 namespace cartographer {
 namespace io {
 
+// std::unique_ptr for Cairo surfaces. The surface is destroyed when the
+// std::unique_ptr is reset or destroyed.
+using UniqueCairoSurfacePtr =
+    std::unique_ptr<cairo_surface_t, void (*)(cairo_surface_t*)>;
+
+// Takes ownership.
+UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface);
+
+// std::unique_ptr for Cairo contexts.
+using UniqueCairoPtr = std::unique_ptr<cairo_t, void (*)(cairo_t*)>;
+
+// Takes ownership.
+UniqueCairoPtr MakeUniqueCairoPtr(cairo_t* surface);
+
 class Image {
  public:
   Image(int width, int height);
@@ -19,6 +34,12 @@ class Image {
   void WritePng(FileWriter* const file_writer);
 
  private:
+  // Returns a pointer to a cairo surface that contains the current pixel data.
+  // The 'Image' object must therefore outlive the returned surface object. It
+  // is undefined behavior to call any of the mutating functions while a pointer
+  // to this surface is alive.
+  UniqueCairoSurfacePtr GetCairoSurface();
+
   int width_;
   int height_;
   int stride_;

cartographer/io/points_batch.h

@@ -23,13 +23,11 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/time.h"
+#include "cartographer/io/color.h"
 
 namespace cartographer {
 namespace io {
 
-// A point's color.
-using Color = std::array<uint8_t, 3>;
-
 // A number of points, captured around the same 'time' and by a
 // sensor at the same 'origin'.
 struct PointsBatch {

cartographer/io/xray_points_processor.cc

@@ -26,6 +26,7 @@
 #include "cartographer/io/image.h"
 #include "cartographer/mapping/detect_floors.h"
 #include "cartographer/mapping_3d/hybrid_grid.h"
+#include "cartographer/transform/transform.h"
 
 namespace cartographer {
 namespace io {
@@ -46,9 +47,8 @@ double Mix(const double a, const double b, const double t) {
 }
 
 // Write 'mat' as a pleasing-to-look-at PNG into 'filename'
-void WriteImage(const PixelDataMatrix& mat, FileWriter* const file_writer) {
+Image IntoImage(const PixelDataMatrix& mat) {
   Image image(mat.cols(), mat.rows());
-
   float max = std::numeric_limits<float>::min();
   for (int y = 0; y < mat.rows(); ++y) {
     for (int x = 0; x < mat.cols(); ++x) {
@@ -87,9 +87,7 @@ void WriteImage(const PixelDataMatrix& mat, FileWriter* const file_writer) {
       image.SetPixel(x, y, {{r, g, b}});
     }
   }
-
+  return image;
-  image.WritePng(file_writer);
-  CHECK(file_writer->Close());
 }
 
 bool ContainedIn(const common::Time& time,
@@ -155,12 +153,12 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
   // For the screen we are using. X: right, Y: up
   const int xsize = bounding_box_.sizes()[1] + 1;
   const int ysize = bounding_box_.sizes()[2] + 1;
-  PixelDataMatrix image = PixelDataMatrix(ysize, xsize);
+  PixelDataMatrix pixel_data_matrix = PixelDataMatrix(ysize, xsize);
   for (mapping_3d::HybridGridBase<bool>::Iterator it(aggregation.voxels);
        !it.Done(); it.Next()) {
     const Eigen::Array3i cell_index = it.GetCellIndex();
     const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index);
-    PixelData& pixel_data = image(pixel.y(), pixel.x());
+    PixelData& pixel_data = pixel_data_matrix(pixel.y(), pixel.x());
     const auto& column_data = aggregation.column_data.at(
         std::make_pair(cell_index[1], cell_index[2]));
     pixel_data.mean_r = column_data.sum_r / column_data.count;
@@ -168,15 +166,19 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
     pixel_data.mean_b = column_data.sum_b / column_data.count;
     ++pixel_data.num_occupied_cells_in_column;
   }
-  WriteImage(image, file_writer);
+  Image image = IntoImage(pixel_data_matrix);
+
+  // TODO(hrapp): Draw trajectories here.
+
+  image.WritePng(file_writer);
+  CHECK(file_writer->Close());
 }
 
 void XRayPointsProcessor::Insert(const PointsBatch& batch,
-                                 const transform::Rigid3f& transform,
                                  Aggregation* const aggregation) {
   constexpr Color kDefaultColor = {{0, 0, 0}};
   for (size_t i = 0; i < batch.points.size(); ++i) {
-    const Eigen::Vector3f camera_point = transform * batch.points[i];
+    const Eigen::Vector3f camera_point = transform_ * batch.points[i];
     const Eigen::Array3i cell_index =
         aggregation->voxels.GetCellIndex(camera_point);
     *aggregation->voxels.mutable_value(cell_index) = true;
@@ -195,13 +197,13 @@ void XRayPointsProcessor::Insert(const PointsBatch& batch,
 void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
   if (floors_.empty()) {
     CHECK_EQ(aggregations_.size(), 1);
-    Insert(*batch, transform_, &aggregations_[0]);
+    Insert(*batch, &aggregations_[0]);
   } else {
     for (size_t i = 0; i < floors_.size(); ++i) {
       if (!ContainedIn(batch->time, floors_[i].timespans)) {
         continue;
       }
-      Insert(*batch, transform_, &aggregations_[i]);
+      Insert(*batch, &aggregations_[i]);
     }
   }
   next_->Process(std::move(batch));

cartographer/io/xray_points_processor.h

@@ -69,8 +69,7 @@ class XRayPointsProcessor : public PointsProcessor {
 
   void WriteVoxels(const Aggregation& aggregation,
                    FileWriter* const file_writer);
-  void Insert(const PointsBatch& batch, const transform::Rigid3f& transform,
+  void Insert(const PointsBatch& batch, Aggregation* aggregation);
-              Aggregation* aggregation);
 
   PointsProcessor* const next_;
   FileWriterFactory file_writer_factory_;