From dc745a147a02548c21928c93423ae44f3e863d9b Mon Sep 17 00:00:00 2001
From: jie <zhengj@google.com>
Date: Tue, 2 Apr 2019 07:57:27 -0700
Subject: [PATCH] Add saturation_factor to scale the saturation of the point
 color (#1556)

Add saturation_factor to scale the saturation of the point color to get darker XRay for sparse point cloud.
---
 cartographer/io/xray_points_processor.cc | 19 +++++++++++++------
 cartographer/io/xray_points_processor.h  |  7 ++++++-
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc
index 4612e66..282bdda 100644
--- a/cartographer/io/xray_points_processor.cc
+++ b/cartographer/io/xray_points_processor.cc
@@ -67,7 +67,7 @@ float Mix(const float a, const float b, const float t) {
 }
 
 // Convert 'matrix' into a pleasing-to-look-at image.
-Image IntoImage(const PixelDataMatrix& matrix) {
+Image IntoImage(const PixelDataMatrix& matrix, double saturation_factor) {
   Image image(matrix.width(), matrix.height());
   float max = std::numeric_limits<float>::min();
   for (int y = 0; y < matrix.height(); ++y) {
@@ -92,7 +92,8 @@ Image IntoImage(const PixelDataMatrix& matrix) {
       // 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;
+          std::min<float>(1.0, std::log(cell.num_occupied_cells_in_column) /
+                                   max * saturation_factor);
       const FloatColor color = {{Mix(1.f, cell.mean_r, saturation),
                                  Mix(1.f, cell.mean_g, saturation),
                                  Mix(1.f, cell.mean_b, saturation)}};
@@ -115,7 +116,8 @@ bool ContainedIn(const common::Time& time,
 }  // namespace
 
 XRayPointsProcessor::XRayPointsProcessor(
-    const double voxel_size, const transform::Rigid3f& transform,
+    const double voxel_size, const double saturation_factor,
+    const transform::Rigid3f& transform,
     const std::vector<mapping::Floor>& floors,
     const DrawTrajectories& draw_trajectories,
     const std::string& output_filename,
@@ -127,7 +129,8 @@ XRayPointsProcessor::XRayPointsProcessor(
       next_(next),
       floors_(floors),
       output_filename_(output_filename),
-      transform_(transform) {
+      transform_(transform),
+      saturation_factor_(saturation_factor) {
   for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
     aggregations_.emplace_back(
         Aggregation{mapping::HybridGridBase<bool>(voxel_size), {}});
@@ -146,6 +149,10 @@ std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
                                   dictionary->GetBool("draw_trajectories"))
                                      ? DrawTrajectories::kYes
                                      : DrawTrajectories::kNo;
+  const double saturation_factor =
+      dictionary->HasKey("saturation_factor")
+          ? dictionary->GetDouble("saturation_factor")
+          : 1.;
   if (separate_floor) {
     CHECK_EQ(trajectories.size(), 1)
         << "Can only detect floors with a single trajectory.";
@@ -153,7 +160,7 @@ std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
   }
 
   return absl::make_unique<XRayPointsProcessor>(
-      dictionary->GetDouble("voxel_size"),
+      dictionary->GetDouble("voxel_size"), saturation_factor,
       transform::FromDictionary(dictionary->GetDictionary("transform").get())
           .cast<float>(),
       floors, draw_trajectories, dictionary->GetString("filename"),
@@ -193,7 +200,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
     ++pixel_data.num_occupied_cells_in_column;
   }
 
-  Image image = IntoImage(pixel_data_matrix);
+  Image image = IntoImage(pixel_data_matrix, saturation_factor_);
   if (draw_trajectories_ == DrawTrajectories::kYes) {
     for (size_t i = 0; i < trajectories_.size(); ++i) {
       DrawTrajectory(
diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h
index 1d269a8..8d0f758 100644
--- a/cartographer/io/xray_points_processor.h
+++ b/cartographer/io/xray_points_processor.h
@@ -38,7 +38,8 @@ class XRayPointsProcessor : public PointsProcessor {
       "write_xray_image";
   enum class DrawTrajectories { kNo, kYes };
   XRayPointsProcessor(
-      double voxel_size, const transform::Rigid3f& transform,
+      double voxel_size, double saturation_factor,
+      const transform::Rigid3f& transform,
       const std::vector<mapping::Floor>& floors,
       const DrawTrajectories& draw_trajectories,
       const std::string& output_filename,
@@ -90,6 +91,10 @@ class XRayPointsProcessor : public PointsProcessor {
 
   // Bounding box containing all cells with data in all 'aggregations_'.
   Eigen::AlignedBox3i bounding_box_;
+
+  // Scale the saturation of the point color. If saturation_factor_ > 1, the
+  // point has darker color, otherwise it has lighter color.
+  const double saturation_factor_;
 };
 
 }  // namespace io