Make intensities a separate channel in PointsBatch. (#187)

And add a PointsProcessor that translates from intensities to RGB.

Fixes #171.

cartographer/io/coloring_points_processor.cc

@@ -30,7 +30,9 @@ ColoringPointsProcessor::FromDictionary(
   const string frame_id = dictionary->GetString("frame_id");
   const std::vector<double> color_values =
       dictionary->GetDictionary("color")->GetArrayValuesAsDoubles();
-  Color color = {{color_values[0], color_values[1], color_values[2]}};
+  const Color color = {{static_cast<uint8_t>(color_values[0]),
+                        static_cast<uint8_t>(color_values[1]),
+                        static_cast<uint8_t>(color_values[2])}};
   return common::make_unique<ColoringPointsProcessor>(color, frame_id, next);
 }
 
@@ -42,7 +44,7 @@ ColoringPointsProcessor::ColoringPointsProcessor(const Color& color,
 void ColoringPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
   if (batch->frame_id == frame_id_) {
     batch->colors.clear();
-    for (int i = 0; i < batch->points.size(); ++i) {
+    for (size_t i = 0; i < batch->points.size(); ++i) {
       batch->colors.push_back(color_);
     }
   }

cartographer/io/intensity_to_color_points_processor.cc

@@ -0,0 +1,69 @@
+/*
+ * 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/intensity_to_color_points_processor.h"
+
+#include "Eigen/Core"
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/math.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+
+std::unique_ptr<IntensityToColorPointsProcessor>
+IntensityToColorPointsProcessor::FromDictionary(
+    common::LuaParameterDictionary* const dictionary,
+    PointsProcessor* const next) {
+  const string frame_id =
+      dictionary->HasKey("frame_id") ? dictionary->GetString("frame_id") : "";
+  const float min_intensity = dictionary->GetDouble("min_intensity");
+  const float max_intensity = dictionary->GetDouble("max_intensity");
+  return common::make_unique<IntensityToColorPointsProcessor>(
+      min_intensity, max_intensity, frame_id, next);
+}
+
+IntensityToColorPointsProcessor::IntensityToColorPointsProcessor(
+    const float min_intensity, const float max_intensity,
+    const string& frame_id, PointsProcessor* const next)
+    : min_intensity_(min_intensity),
+      max_intensity_(max_intensity),
+      frame_id_(frame_id),
+      next_(next) {}
+
+void IntensityToColorPointsProcessor::Process(
+    std::unique_ptr<PointsBatch> batch) {
+  if (!batch->intensities.empty() &&
+      (frame_id_.empty() || batch->frame_id == frame_id_)) {
+    batch->colors.clear();
+    for (const float intensity : batch->intensities) {
+      const uint8_t gray =
+          cartographer::common::Clamp(
+              (intensity - min_intensity_) / (max_intensity_ - min_intensity_),
+              0.f, 1.f) *
+          255;
+      batch->colors.push_back(Color{{gray, gray, gray}});
+    }
+  }
+  next_->Process(std::move(batch));
+}
+
+PointsProcessor::FlushResult IntensityToColorPointsProcessor::Flush() {
+  return next_->Flush();
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/intensity_to_color_points_processor.h

@@ -0,0 +1,64 @@
+/*
+ * 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_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_
+
+#include <memory>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/io/points_batch.h"
+#include "cartographer/io/points_processor.h"
+
+namespace cartographer {
+namespace io {
+
+class IntensityToColorPointsProcessor : public PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName =
+      "intensity_to_color";
+
+  // Applies ('intensity' - min ) / (max - min) * 255 and color the point grey
+  // with this value for each point that comes from the sensor with 'frame_id'.
+  // If 'frame_id' is empty, this applies to all points.
+  IntensityToColorPointsProcessor(float min_intensity, float max_intensity,
+                                  const string& frame_id,
+                                  PointsProcessor* next);
+
+  static std::unique_ptr<IntensityToColorPointsProcessor> FromDictionary(
+      common::LuaParameterDictionary* dictionary, PointsProcessor* next);
+
+  ~IntensityToColorPointsProcessor() override{};
+
+  IntensityToColorPointsProcessor(const IntensityToColorPointsProcessor&) =
+      delete;
+  IntensityToColorPointsProcessor& operator=(
+      const IntensityToColorPointsProcessor&) = delete;
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  const float min_intensity_;
+  const float max_intensity_;
+  const string frame_id_;
+  PointsProcessor* const next_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_

cartographer/io/points_batch.cc

@@ -26,6 +26,9 @@ void RemovePoints(std::vector<int> to_remove, PointsBatch* batch) {
     if (!batch->colors.empty()) {
       batch->colors.erase(batch->colors.begin() + index);
     }
+    if (!batch->intensities.empty()) {
+      batch->intensities.erase(batch->intensities.begin() + index);
+    }
   }
 }
 

cartographer/io/points_batch.h

@@ -51,7 +51,17 @@ struct PointsBatch {
   // Trajectory index that produced this point.
   int trajectory_index;
 
+  // Geometry of the points in a metric frame.
   std::vector<Eigen::Vector3f> points;
+
+  // Intensities are optional and may be unspecified. The meaning of these
+  // intensity values varies by device. For example, the VLP16 provides values
+  // in the range [0, 100] for non-specular return values and values up to 255
+  // for specular returns. On the other hand, Hokuyo lasers provide a 16-bit
+  // value that rarely peaks above 4096.
+  std::vector<float> intensities;
+
+  // Colors are optional. If set, they are RGB values.
   std::vector<Color> colors;
 };
 

cartographer/io/points_processor_pipeline_builder.cc

@@ -20,6 +20,7 @@
 #include "cartographer/io/coloring_points_processor.h"
 #include "cartographer/io/counting_points_processor.h"
 #include "cartographer/io/fixed_ratio_sampling_points_processor.h"
+#include "cartographer/io/intensity_to_color_points_processor.h"
 #include "cartographer/io/min_max_range_filtering_points_processor.h"
 #include "cartographer/io/null_points_processor.h"
 #include "cartographer/io/outlier_removing_points_processor.h"
@@ -66,6 +67,7 @@ void RegisterBuiltInPointsProcessors(
   RegisterPlainPointsProcessor<MinMaxRangeFiteringPointsProcessor>(builder);
   RegisterPlainPointsProcessor<OutlierRemovingPointsProcessor>(builder);
   RegisterPlainPointsProcessor<ColoringPointsProcessor>(builder);
+  RegisterPlainPointsProcessor<IntensityToColorPointsProcessor>(builder);
   RegisterFileWritingPointsProcessor<PcdWritingPointsProcessor>(
       file_writer_factory, builder);
   RegisterFileWritingPointsProcessor<PlyWritingPointsProcessor>(