Adds a HybridGridPoints processor (#323)

cartographer/io/hybrid_grid_points_processor.cc

@@ -0,0 +1,67 @@
+#include "cartographer/io/hybrid_grid_points_processor.h"
+
+#include <memory>
+#include <string>
+
+#include "Eigen/Core"
+#include "cartographer/common/make_unique.h"
+#include "cartographer/io/file_writer.h"
+#include "cartographer/io/points_batch.h"
+#include "cartographer/io/points_processor.h"
+#include "cartographer/mapping_3d/hybrid_grid.h"
+#include "cartographer/mapping_3d/range_data_inserter.h"
+#include "cartographer/sensor/range_data.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+
+HybridGridPointsProcessor::HybridGridPointsProcessor(
+    const double voxel_size,
+    const mapping_3d::proto::RangeDataInserterOptions&
+        range_data_inserter_options,
+    std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
+    : next_(next),
+      range_data_inserter_(range_data_inserter_options),
+      hybrid_grid_(voxel_size),
+      file_writer_(std::move(file_writer)) {}
+
+std::unique_ptr<HybridGridPointsProcessor>
+HybridGridPointsProcessor::FromDictionary(
+    FileWriterFactory file_writer_factory,
+    common::LuaParameterDictionary* const dictionary,
+    PointsProcessor* const next) {
+  return common::make_unique<HybridGridPointsProcessor>(
+      dictionary->GetDouble("voxel_size"),
+      mapping_3d::CreateRangeDataInserterOptions(
+          dictionary->GetDictionary("range_data_inserter").get()),
+      file_writer_factory(dictionary->GetString("filename")), next);
+}
+
+void HybridGridPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
+  range_data_inserter_.Insert({batch->origin, batch->points, {}},
+                              &hybrid_grid_);
+  next_->Process(std::move(batch));
+}
+
+PointsProcessor::FlushResult HybridGridPointsProcessor::Flush() {
+  const mapping_3d::proto::HybridGrid hybrid_grid_proto =
+      mapping_3d::ToProto(hybrid_grid_);
+  string serialized;
+  hybrid_grid_proto.SerializeToString(&serialized);
+  file_writer_->Write(serialized.data(), serialized.size());
+  CHECK(file_writer_->Close());
+
+  switch (next_->Flush()) {
+    case FlushResult::kRestartStream:
+      LOG(FATAL) << "Hybrid grid generation must be configured to occur after "
+                    "any stages that require multiple passes.";
+
+    case FlushResult::kFinished:
+      return FlushResult::kFinished;
+  }
+  LOG(FATAL) << "Failed to receive FlushResult::kFinished";
+}
+
+}  // namespace io
+}  // namespace cartographer

cartographer/io/hybrid_grid_points_processor.h

@@ -0,0 +1,55 @@
+#ifndef CARTOGRAPHER_IO_HYBRID_GRID_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_HYBRID_GRID_POINTS_PROCESSOR_H_
+
+// Library used for inserting range data points into a hybrid grid.
+
+#include <memory>
+#include <string>
+
+#include "cartographer/io/file_writer.h"
+#include "cartographer/io/points_batch.h"
+#include "cartographer/io/points_processor.h"
+#include "cartographer/mapping_3d/hybrid_grid.h"
+#include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h"
+#include "cartographer/mapping_3d/range_data_inserter.h"
+
+namespace cartographer {
+namespace io {
+
+// Creates a hybrid grid of the points with voxels being 'voxel_size'
+// big. 'range_data_inserter' options are used to configure the range
+// data ray tracing through the hybrid grid.
+class HybridGridPointsProcessor : public PointsProcessor {
+ public:
+  constexpr static const char* kConfigurationFileActionName =
+      "write_hybrid_grid";
+  HybridGridPointsProcessor(double voxel_size,
+                            const mapping_3d::proto::RangeDataInserterOptions&
+                                range_data_inserter_options,
+                            std::unique_ptr<FileWriter> file_writer,
+                            PointsProcessor* next);
+  HybridGridPointsProcessor(const HybridGridPointsProcessor&) = delete;
+  HybridGridPointsProcessor& operator=(const HybridGridPointsProcessor&) =
+      delete;
+
+  static std::unique_ptr<HybridGridPointsProcessor> FromDictionary(
+      FileWriterFactory file_writer_factory,
+      common::LuaParameterDictionary* dictionary, PointsProcessor* next);
+
+  ~HybridGridPointsProcessor() override {}
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  PointsProcessor* const next_;
+
+  mapping_3d::RangeDataInserter range_data_inserter_;
+  mapping_3d::HybridGrid hybrid_grid_;
+  std::unique_ptr<FileWriter> file_writer_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_HYBRID_GRID_POINTS_PROCESSOR_H_

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/hybrid_grid_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"
@@ -74,6 +75,8 @@ void RegisterBuiltInPointsProcessors(
       file_writer_factory, builder);
   RegisterFileWritingPointsProcessor<XyzWriterPointsProcessor>(
       file_writer_factory, builder);
+  RegisterFileWritingPointsProcessor<HybridGridPointsProcessor>(
+      file_writer_factory, builder);
 
   // X-Ray is an odd ball since it requires the trajectory to figure out the
   // different building levels we walked on to separate the images.