From f2916143ef7de346a440202dd47bb4722da553c4 Mon Sep 17 00:00:00 2001
From: Holger Rapp <HolgerRapp@gmx.net>
Date: Thu, 20 Oct 2016 09:28:18 +0200
Subject: [PATCH] Adds a PointProcessor that writes ASCII files. (#86)

The files will just contain XYZ points in a file of their own. This is
useful for example as interop with AutoCAD.
---
 cartographer/io/CMakeLists.txt                | 10 ++++
 .../io/ply_writing_points_processor.cc        |  3 +-
 .../io/xyz_writing_points_processor.cc        | 46 ++++++++++++++++++
 .../io/xyz_writing_points_processor.h         | 48 +++++++++++++++++++
 4 files changed, 106 insertions(+), 1 deletion(-)
 create mode 100644 cartographer/io/xyz_writing_points_processor.cc
 create mode 100644 cartographer/io/xyz_writing_points_processor.h

diff --git a/cartographer/io/CMakeLists.txt b/cartographer/io/CMakeLists.txt
index 6e4a936..8269e53 100644
--- a/cartographer/io/CMakeLists.txt
+++ b/cartographer/io/CMakeLists.txt
@@ -62,3 +62,13 @@ google_library(io_xray_points_processor
     mapping_3d_hybrid_grid
     transform_rigid_transform
 )
+
+google_library(io_xyz_writing_points_processor
+  USES_GLOG
+  SRCS
+    xyz_writing_points_processor.cc
+  HDRS
+    xyz_writing_points_processor.h
+  DEPENDS
+    io_points_processor
+)
diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc
index 1422996..18731c0 100644
--- a/cartographer/io/ply_writing_points_processor.cc
+++ b/cartographer/io/ply_writing_points_processor.cc
@@ -75,7 +75,8 @@ PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() {
   file_.seekp(0);
 
   WriteBinaryPlyHeader(has_colors_, num_points_, &file_);
-  file_.flush();
+  file_.close();
+  CHECK(file_) << "Writing PLY file failed.";
 
   switch (next_->Flush()) {
     case FlushResult::kFinished:
diff --git a/cartographer/io/xyz_writing_points_processor.cc b/cartographer/io/xyz_writing_points_processor.cc
new file mode 100644
index 0000000..de321ea
--- /dev/null
+++ b/cartographer/io/xyz_writing_points_processor.cc
@@ -0,0 +1,46 @@
+#include "cartographer/io/xyz_writing_points_processor.h"
+
+#include <iomanip>
+
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace io {
+
+namespace {
+
+void WriteXyzPoint(const Eigen::Vector3f& point, std::ofstream* output) {
+  (*output) << point.x() << " " << point.y() << " " << point.z() << "\n";
+}
+
+}  // namespace
+
+XyzWriterPointsProcessor::XyzWriterPointsProcessor(const string& filename,
+                                                   PointsProcessor* next)
+    : next_(next), file_(filename, std::ios_base::out | std::ios_base::binary) {
+  file_ << std::setprecision(6);
+}
+
+PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() {
+  file_.close();
+  CHECK(file_) << "Writing XYZ file failed.";
+  switch (next_->Flush()) {
+    case FlushResult::kFinished:
+      return FlushResult::kFinished;
+
+    case FlushResult::kRestartStream:
+      LOG(FATAL) << "XYZ generation must be configured to occur after any "
+                    "stages that require multiple passes.";
+  }
+  LOG(FATAL);
+}
+
+void XyzWriterPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
+  for (const Eigen::Vector3f& point : batch->points) {
+    WriteXyzPoint(point, &file_);
+  }
+  next_->Process(std::move(batch));
+}
+
+}  // namespace io
+}  // namespace cartographer
diff --git a/cartographer/io/xyz_writing_points_processor.h b/cartographer/io/xyz_writing_points_processor.h
new file mode 100644
index 0000000..761e13d
--- /dev/null
+++ b/cartographer/io/xyz_writing_points_processor.h
@@ -0,0 +1,48 @@
+/*
+ * 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_XYZ_WRITING_POINTS_PROCESSOR_H_
+#define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_
+
+#include <fstream>
+#include <string>
+
+#include "cartographer/io/points_processor.h"
+
+namespace cartographer {
+namespace io {
+
+// Writes ASCII xyz points.
+class XyzWriterPointsProcessor : public PointsProcessor {
+ public:
+  XyzWriterPointsProcessor(const string& filename, PointsProcessor* next);
+  ~XyzWriterPointsProcessor() override {}
+
+  XyzWriterPointsProcessor(const XyzWriterPointsProcessor&) = delete;
+  XyzWriterPointsProcessor& operator=(const XyzWriterPointsProcessor&) = delete;
+
+  void Process(std::unique_ptr<PointsBatch> batch) override;
+  FlushResult Flush() override;
+
+ private:
+  PointsProcessor* const next_;
+  std::ofstream file_;
+};
+
+}  // namespace io
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_