Color X-Rays using the color of points. (#185)

Colorless points are considered to be black. Creating assets with
intensities will give now brighter X-Rays. I plan to fix this by adding
intensities besides colors into PointBatch and having a PointsProcessor
that converts intensities into colors.

cartographer/io/xray_points_processor.cc

@@ -32,20 +32,18 @@ namespace cartographer {
 namespace io {
 namespace {
 
-using Voxels = mapping_3d::HybridGridBase<bool>;
+struct PixelData {
+  size_t num_occupied_cells_in_column = 0;
+  double mean_r = 0.;
+  double mean_g = 0.;
+  double mean_b = 0.;
+};
 
-// Takes the logarithm of each value in 'mat', clamping to 0 as smallest value.
+using PixelDataMatrix =
-void TakeLogarithm(Eigen::MatrixXf* mat) {
+    Eigen::Matrix<PixelData, Eigen::Dynamic, Eigen::Dynamic>;
-  for (int y = 0; y < mat->rows(); ++y) {
+
-    for (int x = 0; x < mat->cols(); ++x) {
+double Mix(const double a, const double b, const double t) {
-      const float value = (*mat)(y, x);
+  return a * (1. - t) + t * b;
-      if (value == 0.f) {
-        continue;
-      }
-      const float new_value = std::log(value);
-      (*mat)(y, x) = new_value;
-    }
-  }
 }
 
 cairo_status_t CairoWriteCallback(void* const closure,
@@ -59,19 +57,49 @@ cairo_status_t CairoWriteCallback(void* const closure,
 }
 
 // Write 'mat' as a pleasing-to-look-at PNG into 'filename'
-void WritePng(const Eigen::MatrixXf& mat, FileWriter* const file_writer) {
+void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) {
   const int stride =
       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.);
 
-  const float max = mat.maxCoeff();
+  float max = std::numeric_limits<float>::min();
   for (int y = 0; y < mat.rows(); ++y) {
     for (int x = 0; x < mat.cols(); ++x) {
-      const float value = mat(y, x);
+      const PixelData& cell = mat(y, x);
-      uint8_t shade = common::RoundToInt(255.f * (1.f - value / max));
+      if (cell.num_occupied_cells_in_column == 0.) {
+        continue;
+      }
+      max = std::max<float>(max, std::log(cell.num_occupied_cells_in_column));
+    }
+  }
+
+  for (int y = 0; y < mat.rows(); ++y) {
+    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] =
-          (255 << 24) | (shade << 16) | (shade << 8) | shade;
+            (255 << 24) | (255 << 16) | (255 << 8) | 255;
+        continue;
+      }
+
+      // We use a logarithmic weighting for how saturated a pixel will be. The
+      // basic idea here was that walls (full height) are fully saturated, but
+      // details like chairs and tables are still well visible.
+      const float saturation =
+          std::log(cell.num_occupied_cells_in_column) / max;
+      double mean_r_in_column = (cell.mean_r / 255.);
+      double mean_g_in_column = (cell.mean_g / 255.);
+      double mean_b_in_column = (cell.mean_b / 255.);
+
+      double mix_r = Mix(1., mean_r_in_column, saturation);
+      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 int g = common::RoundToInt(mix_g * 255.);
+      const int b = common::RoundToInt(mix_b * 255.);
+      pixels[y * stride / 4 + x] = (255 << 24) | (r << 16) | (g << 8) | b;
     }
   }
 
@@ -92,40 +120,6 @@ void WritePng(const Eigen::MatrixXf& mat, FileWriter* const file_writer) {
   CHECK(file_writer->Close());
 }
 
-void WriteVoxels(const Voxels& voxels, FileWriter* const file_writer) {
-  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(image, file_writer);
-}
-
 bool ContainedIn(
     const common::Time& time,
     const std::vector<common::Interval<common::Time>>& time_intervals) {
@@ -137,14 +131,6 @@ bool ContainedIn(
   return false;
 }
 
-void Insert(const PointsBatch& batch, const transform::Rigid3f& transform,
-            Voxels* voxels) {
-  for (const auto& point : batch.points) {
-    const Eigen::Vector3f camera_point = transform * point;
-    *voxels->mutable_value(voxels->GetCellIndex(camera_point)) = true;
-  }
-}
-
 }  // namespace
 
 XRayPointsProcessor::XRayPointsProcessor(
@@ -157,7 +143,9 @@ XRayPointsProcessor::XRayPointsProcessor(
       output_filename_(output_filename),
       transform_(transform) {
   for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
-    voxels_.emplace_back(voxel_size, Eigen::Vector3f::Zero());
+    aggregations_.emplace_back(Aggregation{
+        mapping_3d::HybridGridBase<bool>(voxel_size, Eigen::Vector3f::Zero()),
+        {}});
   }
 }
 
@@ -179,16 +167,79 @@ std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
       floors, dictionary->GetString("filename"), file_writer_factory, next);
 }
 
+void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
+                                      FileWriter* const file_writer) {
+  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 (mapping_3d::HybridGridBase<bool>::Iterator it(aggregation.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;
+  PixelDataMatrix image = 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());
+    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;
+    pixel_data.mean_g = column_data.sum_g / column_data.count;
+    pixel_data.mean_b = column_data.sum_b / column_data.count;
+    ++pixel_data.num_occupied_cells_in_column;
+  }
+  WritePng(image, file_writer);
+}
+
+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::Array3i cell_index =
+        aggregation->voxels.GetCellIndex(camera_point);
+    *aggregation->voxels.mutable_value(cell_index) = true;
+    ColumnData& column_data =
+        aggregation->column_data[std::make_pair(cell_index[1], cell_index[2])];
+    const auto& color =
+        batch.colors.empty() ? kDefaultColor : batch.colors.at(i);
+    column_data.sum_r += color[0];
+    column_data.sum_g += color[1];
+    column_data.sum_b += color[2];
+    ++column_data.count;
+  }
+}
+
 void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
   if (floors_.empty()) {
-    CHECK_EQ(voxels_.size(), 1);
+    CHECK_EQ(aggregations_.size(), 1);
-    Insert(*batch, transform_, &voxels_[0]);
+    Insert(*batch, transform_, &aggregations_[0]);
   } else {
     for (size_t i = 0; i < floors_.size(); ++i) {
       if (!ContainedIn(batch->time, floors_[i].timespans)) {
         continue;
       }
-      Insert(*batch, transform_, &voxels_[i]);
+      Insert(*batch, transform_, &aggregations_[i]);
     }
   }
   next_->Process(std::move(batch));
@@ -196,13 +247,13 @@ void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
 
 PointsProcessor::FlushResult XRayPointsProcessor::Flush() {
   if (floors_.empty()) {
-    CHECK_EQ(voxels_.size(), 1);
+    CHECK_EQ(aggregations_.size(), 1);
-    WriteVoxels(voxels_[0],
+    WriteVoxels(aggregations_[0],
                 file_writer_factory_(output_filename_ + ".png").get());
   } else {
     for (size_t i = 0; i < floors_.size(); ++i) {
       WriteVoxels(
-          voxels_[i],
+          aggregations_[i],
           file_writer_factory_(output_filename_ + std::to_string(i) + ".png")
               .get());
     }

cartographer/io/xray_points_processor.h

@@ -17,6 +17,9 @@
 #ifndef CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
 #define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
 
+#include <map>
+
+#include "Eigen/Core"
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/io/file_writer.h"
 #include "cartographer/io/points_processor.h"
@@ -50,6 +53,22 @@ class XRayPointsProcessor : public PointsProcessor {
   FlushResult Flush() override;
 
  private:
+  struct ColumnData {
+    double sum_r = 0.;
+    double sum_g = 0.;
+    double sum_b = 0.;
+    uint32_t count = 0;
+  };
+
+  struct Aggregation {
+    mapping_3d::HybridGridBase<bool> voxels;
+    std::map<std::pair<int, int>, ColumnData> column_data;
+  };
+
+  void WriteVoxels(const Aggregation& aggregation, FileWriter* const file_writer);
+  void Insert(const PointsBatch& batch, const transform::Rigid3f& transform,
+              Aggregation* aggregation);
+
   PointsProcessor* const next_;
   const FileWriterFactory& file_writer_factory_;
 
@@ -60,7 +79,7 @@ class XRayPointsProcessor : public PointsProcessor {
   const transform::Rigid3f transform_;
 
   // Only has one entry if we do not separate into floors.
-  std::vector<mapping_3d::HybridGridBase<bool>> voxels_;
+  std::vector<Aggregation> aggregations_;
 };
 
 }  // namespace io