Adds a PointCloudProcessor that colors points by frame_id. (#184)

cartographer/io/coloring_points_processor.cc

@@ -0,0 +1,57 @@
+/*
+ * 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/coloring_points_processor.h"
+
+#include "Eigen/Core"
+#include "cartographer/common/make_unique.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+
+std::unique_ptr<ColoringPointsProcessor>
+ColoringPointsProcessor::FromDictionary(
+    common::LuaParameterDictionary* const dictionary,
+    PointsProcessor* const next) {
+  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]}};
+  return common::make_unique<ColoringPointsProcessor>(color, frame_id, next);
+}
+
+ColoringPointsProcessor::ColoringPointsProcessor(const Color& color,
+                                                 const string& frame_id,
+                                                 PointsProcessor* const next)
+    : color_(color), frame_id_(frame_id), next_(next) {}
+
+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) {
+      batch->colors.push_back(color_);
+    }
+  }
+  next_->Process(std::move(batch));
+}
+
+PointsProcessor::FlushResult ColoringPointsProcessor::Flush() {
+  return next_->Flush();
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/coloring_points_processor.h

@@ -0,0 +1,57 @@
+/*
+ * 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_COLORING_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_COLORING_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 {
+
+// Colors points with a fixed color by frame_id.
+class ColoringPointsProcessor : public PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName = "color_points";
+
+  ColoringPointsProcessor(const Color& color, const string& frame_id,
+                          PointsProcessor* next);
+
+  static std::unique_ptr<ColoringPointsProcessor> FromDictionary(
+      common::LuaParameterDictionary* dictionary, PointsProcessor* next);
+
+  ~ColoringPointsProcessor() override{};
+
+  ColoringPointsProcessor(const ColoringPointsProcessor&) = delete;
+  ColoringPointsProcessor& operator=(const ColoringPointsProcessor&) = delete;
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  const Color color_;
+  const string frame_id_;
+  PointsProcessor* const next_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_

cartographer/io/points_processor_pipeline_builder.cc

@@ -17,6 +17,7 @@
 #include "cartographer/io/points_processor_pipeline_builder.h"
 
 #include "cartographer/common/make_unique.h"
+#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/min_max_range_filtering_points_processor.h"
@@ -64,6 +65,7 @@ void RegisterBuiltInPointsProcessors(
   RegisterPlainPointsProcessor<FixedRatioSamplingPointsProcessor>(builder);
   RegisterPlainPointsProcessor<MinMaxRangeFiteringPointsProcessor>(builder);
   RegisterPlainPointsProcessor<OutlierRemovingPointsProcessor>(builder);
+  RegisterPlainPointsProcessor<ColoringPointsProcessor>(builder);
   RegisterFileWritingPointsProcessor<PcdWritingPointsProcessor>(
       file_writer_factory, builder);
   RegisterFileWritingPointsProcessor<PlyWritingPointsProcessor>(