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

@@ -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
+)

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:

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

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_