Adds a points processing pipeline and a XRay creator. (#66)

- Adds cairo as required dependency.
- Adds an 'io' module.

CMakeLists.txt

@@ -25,11 +25,14 @@ include("${CMAKE_SOURCE_DIR}/cmake/functions.cmake")
 google_initialize_cartographer_project()
 google_enable_testing()
 
+include(FindPkgConfig)
+
 find_package(Boost REQUIRED COMPONENTS system iostreams)
 find_package(Ceres REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(LuaGoogle REQUIRED)
 find_package(Protobuf REQUIRED)
+PKG_SEARCH_MODULE(CAIRO REQUIRED cairo>=1.12.16)
 
 # Only build the documentation if we can find Sphinx.
 find_package(Sphinx)
@@ -83,6 +86,7 @@ list(APPEND CARTOGRAPHER_LIBRARIES "${CERES_LIBRARIES}")
 list(APPEND CARTOGRAPHER_LIBRARIES "${Boost_LIBRARIES}")
 list(APPEND CARTOGRAPHER_LIBRARIES "${LUA_LIBRARIES}")
 list(APPEND CARTOGRAPHER_LIBRARIES "${PROTOBUF_LIBRARIES}")
+list(APPEND CARTOGRAPHER_LIBRARIES "${CAIRO_LIBRARIES}")
 list(APPEND CARTOGRAPHER_LIBRARIES "webp")
 
 CONFIGURE_PACKAGE_CONFIG_FILE(

cartographer/CMakeLists.txt

@@ -13,6 +13,7 @@
 # limitations under the License.
 
 add_subdirectory("common")
+add_subdirectory("io")
 add_subdirectory("kalman_filter")
 add_subdirectory("mapping")
 add_subdirectory("mapping_2d")

cartographer/io/CMakeLists.txt

@@ -0,0 +1,42 @@
+google_library(io_cairo_types
+  USES_CAIRO
+  HDRS
+    cairo_types.h
+)
+
+google_library(io_null_points_processor
+  HDRS
+    null_points_processor.h
+  DEPENDS
+    io_points_processor
+)
+
+google_library(io_points_batch
+  USES_EIGEN
+  HDRS
+    points_batch.h
+  DEPENDS
+    common_time
+)
+
+google_library(io_points_processor
+  HDRS
+    points_processor.h
+  DEPENDS
+    io_points_batch
+)
+
+google_library(io_xray_points_processor
+  USES_CAIRO
+  USES_EIGEN
+  SRCS
+    xray_points_processor.cc
+  HDRS
+    xray_points_processor.h
+  DEPENDS
+    common_math
+    io_cairo_types
+    io_points_processor
+    mapping_3d_hybrid_grid
+    transform_rigid_transform
+)

cartographer/io/cairo_types.h

@@ -0,0 +1,29 @@
+#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/null_points_processor.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2016 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_NULL_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_
+
+#include "cartographer/io/points_processor.h"
+
+namespace cartographer {
+namespace io {
+
+// A points processor that just drops all points. The end of a pipeline usually.
+class NullPointsProcessor : public PointsProcessor {
+ public:
+  NullPointsProcessor() {}
+  ~NullPointsProcessor() override {}
+
+  void Process(const PointsBatch& points_batch) override {}
+  void Flush() override {}
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_

cartographer/io/points_batch.h

@@ -0,0 +1,58 @@
+/*
+ * Copyright 2016 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_POINTS_BATCH_H_
+#define CARTOGRAPHER_IO_POINTS_BATCH_H_
+
+#include <cstdint>
+
+#include "Eigen/Core"
+#include "cartographer/common/time.h"
+
+namespace cartographer {
+namespace io {
+
+// A number of points, captured around the same 'time' and by a
+// sensor at the same 'origin'.
+struct PointsBatch {
+  PointsBatch() {
+    origin = Eigen::Vector3f::Zero();
+    trajectory_index = 0;
+  }
+
+  // Time at which this batch has been acquired.
+  common::Time time;
+
+  // Origin of the data, i.e. the location of the sensor in the world at
+  // 'time'.
+  Eigen::Vector3f origin;
+
+  // Sensor that generated this data's 'frame_id' or empty if this information
+  // is unknown.
+  string frame_id;
+
+  // Trajectory index that produced this point.
+  int trajectory_index;
+
+  std::vector<Eigen::Vector3f> points;
+  std::vector<Eigen::Vector3f> normals;
+  std::vector<Eigen::Matrix<uint8_t, 3, 1>> colors;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_POINTS_BATCH_H_

cartographer/io/points_processor.h

@@ -0,0 +1,43 @@
+/*
+ * Copyright 2016 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_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_
+
+#include "cartographer/io/points_batch.h"
+
+namespace cartographer {
+namespace io {
+
+// A processor in a pipeline. It processes a 'points_batch' and hands it to the
+// next processor in the pipeline. Once 'flush' is called no more data will be
+// send through the pipeline.
+class PointsProcessor {
+ public:
+  PointsProcessor() {}
+  virtual ~PointsProcessor() {}
+
+  PointsProcessor(const PointsProcessor&) = delete;
+  PointsProcessor& operator=(const PointsProcessor&) = delete;
+
+  virtual void Process(const PointsBatch& points_batch) = 0;
+  virtual void Flush() = 0;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_POINTS_PROCESSOR_H_

cartographer/io/xray_points_processor.cc

@@ -0,0 +1,134 @@
+/*
+ * Copyright 2016 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/xray_points_processor.h"
+
+#include <cmath>
+#include <string>
+
+#include "Eigen/Core"
+#include "cairo/cairo.h"
+#include "cartographer/common/math.h"
+#include "cartographer/io/cairo_types.h"
+#include "cartographer/mapping_3d/hybrid_grid.h"
+
+namespace cartographer {
+namespace io {
+namespace {
+
+// Takes the logarithm of each value in 'mat', clamping to 0 as smallest value.
+void TakeLogarithm(Eigen::MatrixXf* mat) {
+  for (int y = 0; y < mat->rows(); ++y) {
+    for (int x = 0; x < mat->cols(); ++x) {
+      const float value = (*mat)(y, x);
+      if (value == 0.f) {
+        continue;
+      }
+      const float new_value = std::log(value);
+      (*mat)(y, x) = new_value;
+    }
+  }
+}
+
+// Write 'mat' as a pleasing-to-look-at PNG into 'filename'
+void WritePng(const string& filename, const Eigen::MatrixXf& mat) {
+  const int stride =
+      cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols());
+  std::vector<uint32_t> pixels(stride * mat.rows(), 0.);
+
+  const float max = mat.maxCoeff();
+  for (int y = 0; y < mat.rows(); ++y) {
+    for (int x = 0; x < mat.cols(); ++x) {
+      const float value = mat(y, x);
+      uint8_t shade = common::RoundToInt(255.f * (1.f - value / max));
+      pixels[y * stride / 4 + x] =
+          (255 << 24) | (shade << 16) | (shade << 8) | shade;
+    }
+  }
+
+  // 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.
+  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);
+  cairo_surface_write_to_png(surface.get(), filename.c_str());
+}
+
+}  // namespace
+
+XRayPointsProcessor::XRayPointsProcessor(const double voxel_size,
+                                         const transform::Rigid3f& transform,
+                                         const string& output_filename,
+                                         PointsProcessor* next)
+    : next_(next),
+      output_filename_(output_filename),
+      transform_(transform),
+      voxels_(voxel_size, Eigen::Vector3f::Zero()) {}
+
+void XRayPointsProcessor::Process(const PointsBatch& batch) {
+  for (const auto& point : batch.points) {
+    const Eigen::Vector3f camera_point = transform_ * point;
+    *voxels_.mutable_value(voxels_.GetCellIndex(camera_point)) = true;
+  }
+  next_->Process(batch);
+}
+
+void XRayPointsProcessor::Flush() {
+  WriteImage();
+  return next_->Flush();
+}
+
+void XRayPointsProcessor::WriteImage() {
+  Eigen::Array3i min(std::numeric_limits<int>::max(),
+                     std::numeric_limits<int>::max(),
+                     std::numeric_limits<int>::max());
+  Eigen::Array3i max(std::numeric_limits<int>::min(),
+                     std::numeric_limits<int>::min(),
+                     std::numeric_limits<int>::min());
+
+  // Find the maximum and minimum cells.
+  for (Voxels::Iterator it(voxels_); !it.Done(); it.Next()) {
+    const Eigen::Array3i idx = it.GetCellIndex();
+    min = min.min(idx);
+    max = max.max(idx);
+  }
+
+  // Returns the (x, y) pixel of the given 'index'.
+  const auto voxel_index_to_pixel = [&max, &min](const Eigen::Array3i& index) {
+    // We flip the y axis, since matrices rows are counted from the top.
+    return Eigen::Array2i(max[1] - index[1], max[2] - index[2]);
+  };
+
+  // Hybrid grid uses X: forward, Y: left, Z: up.
+  // For the screen we are using. X: right, Y: up
+  const int xsize = max[1] - min[1] + 1;
+  const int ysize = max[2] - min[2] + 1;
+  Eigen::MatrixXf image = Eigen::MatrixXf::Zero(ysize, xsize);
+  for (Voxels::Iterator it(voxels_); !it.Done(); it.Next()) {
+    const Eigen::Array2i pixel = voxel_index_to_pixel(it.GetCellIndex());
+    ++image(pixel.y(), pixel.x());
+  }
+  TakeLogarithm(&image);
+  WritePng(output_filename_, image);
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/xray_points_processor.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright 2016 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_XRAY_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
+
+#include "cartographer/io/points_processor.h"
+#include "cartographer/mapping_3d/hybrid_grid.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace io {
+
+// Creates X-ray cuts through the points with pixels being 'voxel_size' big. All
+// images created from a single XRayPointsProcessor have the same dimensions and
+// are centered on the center of the bounding box, so that they can easily be
+// combined into a movie.
+class XRayPointsProcessor : public PointsProcessor {
+ public:
+  XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform,
+                      const string& output_filename, PointsProcessor* next);
+
+  ~XRayPointsProcessor() override {}
+
+  void Process(const PointsBatch& batch) override;
+  void Flush() override;
+
+ private:
+  using Voxels = mapping_3d::HybridGridBase<bool>;
+
+  void WriteImage();
+
+  PointsProcessor* const next_;
+  const string output_filename_;
+  const transform::Rigid3f transform_;
+  Voxels voxels_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_

cmake/functions.cmake

@@ -24,6 +24,7 @@ macro(_parse_arguments ARGS)
     USES_GLOG
     USES_LUA
     USES_WEBP
+    USES_CAIRO
   )
 
   # Options only used by projects using Cartographers cmake files.
@@ -106,6 +107,12 @@ macro(_common_compile_stuff VISIBILITY)
     target_link_libraries("${NAME}" yaml-cpp)
   endif()
 
+  if(ARG_USES_CAIRO)
+    target_include_directories("${NAME}" SYSTEM ${VISIBILITY}
+      "${CAIRO_INCLUDE_DIRS}")
+    target_link_libraries("${NAME}" ${CAIRO_LIBRARIES})
+  endif()
+
   set_target_properties(${NAME} PROPERTIES
     COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
 

scripts/install_debs.sh

@@ -25,6 +25,7 @@ sudo apt-get install -y \
     git \
     google-mock \
     libboost-all-dev \
+    libcairo2-dev \
     libeigen3-dev \
     libgflags-dev \
     libgoogle-glog-dev \

scripts/update_cmakelists.py

@@ -114,6 +114,8 @@ def ExtractUses(project_name, source):
       uses.add("USES_ROS")
     if re.match(r'^#include ["<]yaml-cpp/', line):
       uses.add("USES_YAMLCPP")
+    if re.match(r'^#include ["<]cairo/', line):
+      uses.add("USES_CAIRO")
     if project_name != "cartographer":
       if re.match(r'^#include ["<]cartographer/', line):
         uses.add("USES_CARTOGRAPHER")