Change Color to Uint8Color and FloatColor (#423)

As almost all computations for color are done in float, use FloatColor
format by default and convert to Uint8Color only when needed.

cartographer/io/color.cc

@@ -31,13 +31,7 @@ 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) {
+FloatColor HsvToRgb(const float h, const float s, const float v) {
-  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;
@@ -47,25 +41,25 @@ Color HsvToRgb(const float h, const float s, const float v) {
   const float t = v * (1.f - (1.f - f) * s);
 
   if (h_i == 0) {
-    return CreateRgba(v, t, p);
+    return {{v, t, p}};
   } else if (h_i == 1) {
-    return CreateRgba(q, v, p);
+    return {{q, v, p}};
   } else if (h_i == 2) {
-    return CreateRgba(p, v, t);
+    return {{p, v, t}};
   } else if (h_i == 3) {
-    return CreateRgba(p, q, v);
+    return {{p, q, v}};
   } else if (h_i == 4) {
-    return CreateRgba(t, p, v);
+    return {{t, p, v}};
   } else if (h_i == 5) {
-    return CreateRgba(v, p, q);
+    return {{v, p, q}};
   } else {
-    return CreateRgba(0.f, 0.f, 0.f);
+    return {{0.f, 0.f, 0.f}};
   }
 }
 
 }  // namespace
 
-Color GetColor(int id) {
+FloatColor 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/

cartographer/io/color.h

@@ -19,15 +19,35 @@
 
 #include <array>
 
+#include "cartographer/common/math.h"
+#include "cartographer/common/port.h"
+
 namespace cartographer {
 namespace io {
 
-// TODO(hrapp): Should probably be represented as floats.
+using Uint8Color = std::array<uint8, 3>;
-using Color = std::array<uint8_t, 3>;
+using FloatColor = std::array<float, 3>;
 
 // A function for on-demand generation of a color palette, with every two
 // direct successors having large contrast.
-Color GetColor(int id);
+FloatColor GetColor(int id);
+
+inline uint8 FloatComponentToUint8(float c) {
+  return static_cast<uint8>(cartographer::common::RoundToInt(
+      cartographer::common::Clamp(c, 0.f, 1.f) * 255));
+}
+
+inline float Uint8ComponentToFloat(uint8 c) { return c / 255.f; }
+
+inline Uint8Color ToUint8Color(const FloatColor& color) {
+  return {{FloatComponentToUint8(color[0]), FloatComponentToUint8(color[1]),
+           FloatComponentToUint8(color[2])}};
+}
+
+inline FloatColor ToFloatColor(const Uint8Color& color) {
+  return {{Uint8ComponentToFloat(color[0]), Uint8ComponentToFloat(color[1]),
+           Uint8ComponentToFloat(color[2])}};
+}
 
 }  // namespace io
 }  // namespace cartographer

cartographer/io/coloring_points_processor.cc

@@ -30,13 +30,14 @@ ColoringPointsProcessor::FromDictionary(
   const string frame_id = dictionary->GetString("frame_id");
   const std::vector<double> color_values =
       dictionary->GetDictionary("color")->GetArrayValuesAsDoubles();
-  const Color color = {{static_cast<uint8_t>(color_values[0]),
+  const Uint8Color color = {{static_cast<uint8>(color_values[0]),
-                        static_cast<uint8_t>(color_values[1]),
+                             static_cast<uint8>(color_values[1]),
-                        static_cast<uint8_t>(color_values[2])}};
+                             static_cast<uint8>(color_values[2])}};
-  return common::make_unique<ColoringPointsProcessor>(color, frame_id, next);
+  return common::make_unique<ColoringPointsProcessor>(ToFloatColor(color),
+                                                      frame_id, next);
 }
 
-ColoringPointsProcessor::ColoringPointsProcessor(const Color& color,
+ColoringPointsProcessor::ColoringPointsProcessor(const FloatColor& color,
                                                  const string& frame_id,
                                                  PointsProcessor* const next)
     : color_(color), frame_id_(frame_id), next_(next) {}

cartographer/io/coloring_points_processor.h

@@ -31,7 +31,7 @@ class ColoringPointsProcessor : public PointsProcessor {
  public:
   constexpr static const char* kConfigurationFileActionName = "color_points";
 
-  ColoringPointsProcessor(const Color& color, const string& frame_id,
+  ColoringPointsProcessor(const FloatColor& color, const string& frame_id,
                           PointsProcessor* next);
 
   static std::unique_ptr<ColoringPointsProcessor> FromDictionary(
@@ -46,7 +46,7 @@ class ColoringPointsProcessor : public PointsProcessor {
   FlushResult Flush() override;
 
  private:
-  const Color color_;
+  const FloatColor color_;
   const string frame_id_;
   PointsProcessor* const next_;
 };

cartographer/io/draw_trajectories.cc

@@ -23,7 +23,7 @@ namespace cartographer {
 namespace io {
 
 void DrawTrajectory(const mapping::proto::Trajectory& trajectory,
-                    const Color& color, PoseToPixelFunction pose_to_pixel,
+                    const FloatColor& color, PoseToPixelFunction pose_to_pixel,
                     cairo_surface_t* surface) {
   if (trajectory.node_size() == 0) {
     return;
@@ -34,8 +34,7 @@ void DrawTrajectory(const mapping::proto::Trajectory& trajectory,
 
   auto cr = ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface));
 
-  cairo_set_source_rgba(cr.get(), color[0] / 255., color[1] / 255.,
+  cairo_set_source_rgba(cr.get(), color[0], color[1], color[2], kAlpha);
-                        color[2] / 255., kAlpha);
   cairo_set_line_width(cr.get(), kTrajectoryWidth);
 
   for (const auto& node : trajectory.node()) {

cartographer/io/draw_trajectories.h

@@ -32,7 +32,7 @@ using PoseToPixelFunction =
 // 'pose_to_pixel' function must translate a trajectory node's position into the
 // pixel on 'surface'.
 void DrawTrajectory(const mapping::proto::Trajectory& trajectory,
-                    const Color& color, PoseToPixelFunction pose_to_pixel,
+                    const FloatColor& color, PoseToPixelFunction pose_to_pixel,
                     cairo_surface_t* surface);
 
 }  // namespace io

cartographer/io/image.cc

@@ -9,6 +9,18 @@ namespace cartographer {
 namespace io {
 namespace {
 
+uint32 Uint8ColorToCairo(const Uint8Color& color) {
+  return static_cast<uint32>(255) << 24 | static_cast<uint32>(color[0]) << 16 |
+         static_cast<uint32>(color[1]) << 8 | color[2];
+}
+
+Uint8Color CairoToUint8Color(uint32 color) {
+  uint8 r = color >> 16;
+  uint8 g = color >> 8;
+  uint8 b = color;
+  return {{r, g, b}};
+}
+
 cairo_status_t CairoWriteCallback(void* const closure,
                                   const unsigned char* data,
                                   const unsigned int length) {
@@ -56,15 +68,12 @@ void Image::WritePng(FileWriter* const file_writer) {
            CAIRO_STATUS_SUCCESS);
 }
 
-const Color Image::GetPixel(int x, int y) const {
+const Uint8Color Image::GetPixel(int x, int y) const {
-  const uint32_t value = pixels_[y * stride_ / 4 + x];
+  return CairoToUint8Color(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) {
+void Image::SetPixel(int x, int y, const Uint8Color& color) {
-  pixels_[y * stride_ / 4 + x] =
+  pixels_[y * stride_ / 4 + x] = Uint8ColorToCairo(color);
-      (255 << 24) | (color[0] << 16) | (color[1] << 8) | color[2];
 }
 
 UniqueCairoSurfacePtr Image::GetCairoSurface() {

cartographer/io/image.h

@@ -5,6 +5,8 @@
 #include <vector>
 
 #include "cairo/cairo.h"
+#include "cartographer/common/port.h"
+#include "cartographer/io/color.h"
 #include "cartographer/io/file_writer.h"
 #include "cartographer/io/points_batch.h"
 
@@ -29,8 +31,8 @@ class Image {
  public:
   Image(int width, int height);
 
-  const Color GetPixel(int x, int y) const;
+  const Uint8Color GetPixel(int x, int y) const;
-  void SetPixel(int x, int y, const Color& color);
+  void SetPixel(int x, int y, const Uint8Color& color);
   void WritePng(FileWriter* const file_writer);
 
   // Returns a pointer to a cairo surface that contains the current pixel data.
@@ -43,7 +45,7 @@ class Image {
   int width_;
   int height_;
   int stride_;
-  std::vector<uint32_t> pixels_;
+  std::vector<uint32> pixels_;
 };
 
 }  // namespace io

cartographer/io/intensity_to_color_points_processor.cc

@@ -50,12 +50,10 @@ void IntensityToColorPointsProcessor::Process(
       (frame_id_.empty() || batch->frame_id == frame_id_)) {
     batch->colors.clear();
     for (const float intensity : batch->intensities) {
-      const uint8_t gray =
+      const float gray = cartographer::common::Clamp(
-          cartographer::common::Clamp(
+          (intensity - min_intensity_) / (max_intensity_ - min_intensity_), 0.f,
-              (intensity - min_intensity_) / (max_intensity_ - min_intensity_),
+          1.f);
-              0.f, 1.f) *
+      batch->colors.push_back({{gray, gray, gray}});
-          255;
-      batch->colors.push_back(Color{{gray, gray, gray}});
     }
   }
   next_->Process(std::move(batch));

cartographer/io/pcd_writing_points_processor.cc

@@ -65,7 +65,7 @@ void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
   CHECK(file_writer->Write(buffer, 12));
 }
 
-void WriteBinaryPcdPointColor(const Color& color,
+void WriteBinaryPcdPointColor(const Uint8Color& color,
                               FileWriter* const file_writer) {
   char buffer[4];
   buffer[0] = color[2];
@@ -121,7 +121,8 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
   for (size_t i = 0; i < batch->points.size(); ++i) {
     WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get());
     if (!batch->colors.empty()) {
-      WriteBinaryPcdPointColor(batch->colors[i], file_writer_.get());
+      WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]),
+                               file_writer_.get());
     }
     ++num_points_;
   }

cartographer/io/ply_writing_points_processor.cc

@@ -61,7 +61,7 @@ void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
   CHECK(file_writer->Write(buffer, 12));
 }
 
-void WriteBinaryPlyPointColor(const Color& color,
+void WriteBinaryPlyPointColor(const Uint8Color& color,
                               FileWriter* const file_writer) {
   CHECK(file_writer->Write(reinterpret_cast<const char*>(color.data()),
                            color.size()));
@@ -120,7 +120,7 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
   for (size_t i = 0; i < batch->points.size(); ++i) {
     WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
     if (has_colors_) {
-      WriteBinaryPlyPointColor(batch->colors[i], file_.get());
+      WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get());
     }
     ++num_points_;
   }

cartographer/io/points_batch.h

@@ -61,7 +61,7 @@ struct PointsBatch {
   std::vector<float> intensities;
 
   // Colors are optional. If set, they are RGB values.
-  std::vector<Color> colors;
+  std::vector<FloatColor> colors;
 };
 
 // Removes the indices in 'to_remove' from 'batch'.

cartographer/io/probability_grid_points_processor.cc

@@ -28,11 +28,11 @@ void WriteGrid(
                                        const Eigen::Array2i& index) {
     if (probability_grid.IsKnown(index)) {
       const float probability = 1.f - probability_grid.GetProbability(index);
-      return static_cast<uint8_t>(
+      return static_cast<uint8>(
           255 * ((probability - mapping::kMinProbability) /
                  (mapping::kMaxProbability - mapping::kMinProbability)));
     } else {
-      constexpr uint8_t kUnknownValue = 128;
+      constexpr uint8 kUnknownValue = 128;
       return kUnknownValue;
     }
   };

cartographer/io/xray_points_processor.cc

@@ -82,10 +82,10 @@ Image IntoImage(const PixelDataMatrix& mat) {
       double mix_g = Mix(1., mean_g_in_column, saturation);
       double mix_b = Mix(1., mean_b_in_column, saturation);
 
-      const uint8_t r = common::RoundToInt(mix_r * 255.);
+      image.SetPixel(
-      const uint8_t g = common::RoundToInt(mix_g * 255.);
+          x, y,
-      const uint8_t b = common::RoundToInt(mix_b * 255.);
+          {{FloatComponentToUint8(mix_r), FloatComponentToUint8(mix_g),
-      image.SetPixel(x, y, {{r, g, b}});
+            FloatComponentToUint8(mix_b)}});
     }
   }
   return image;
@@ -200,7 +200,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
 
 void XRayPointsProcessor::Insert(const PointsBatch& batch,
                                  Aggregation* const aggregation) {
-  constexpr Color kDefaultColor = {{0, 0, 0}};
+  constexpr FloatColor kDefaultColor = {{0.f, 0.f, 0.f}};
   for (size_t i = 0; i < batch.points.size(); ++i) {
     const Eigen::Vector3f camera_point = transform_ * batch.points[i];
     const Eigen::Array3i cell_index =