From 99f79e3f69d4958fcbd797c9d1e8db1abf65e876 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Tue, 17 Jan 2017 12:29:11 +0100 Subject: [PATCH] Adds a FileWriter abstraction (#182) --- cartographer/io/file_writer.cc | 53 ++++++++++++++ cartographer/io/file_writer.h | 68 ++++++++++++++++++ .../io/pcd_writing_points_processor.cc | 72 ++++++++++--------- .../io/pcd_writing_points_processor.h | 7 +- .../io/ply_writing_points_processor.cc | 61 ++++++++-------- .../io/ply_writing_points_processor.h | 9 +-- .../io/points_processor_pipeline_builder.cc | 35 ++++++--- .../io/points_processor_pipeline_builder.h | 2 + cartographer/io/xray_points_processor.cc | 37 +++++++--- cartographer/io/xray_points_processor.h | 7 +- .../io/xyz_writing_points_processor.cc | 25 ++++--- .../io/xyz_writing_points_processor.h | 7 +- 12 files changed, 280 insertions(+), 103 deletions(-) create mode 100644 cartographer/io/file_writer.cc create mode 100644 cartographer/io/file_writer.h diff --git a/cartographer/io/file_writer.cc b/cartographer/io/file_writer.cc new file mode 100644 index 0000000..769763d --- /dev/null +++ b/cartographer/io/file_writer.cc @@ -0,0 +1,53 @@ +/* + * 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/file_writer.h" + +namespace cartographer { +namespace io { + +StreamFileWriter::StreamFileWriter(const string& filename) + : out_(filename, std::ios::out | std::ios::binary) {} + +StreamFileWriter::~StreamFileWriter() {} + +bool StreamFileWriter::Write(const char* const data, const size_t len) { + if (out_.bad()) { + return false; + } + out_.write(data, len); + return !out_.bad(); +} + +bool StreamFileWriter::Close() { + if (out_.bad()) { + return false; + } + out_.close(); + return !out_.bad(); +} + +bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) { + if (out_.bad()) { + return false; + } + out_.flush(); + out_.seekp(0); + return Write(data, len); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/file_writer.h b/cartographer/io/file_writer.h new file mode 100644 index 0000000..e542201 --- /dev/null +++ b/cartographer/io/file_writer.h @@ -0,0 +1,68 @@ +/* + * 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_FILE_WRITER_H_ +#define CARTOGRAPHER_IO_FILE_WRITER_H_ + +#include +#include +#include + +#include "cartographer/common/port.h" + +namespace cartographer { +namespace io { + +// Simple abstraction for a file. +class FileWriter { + public: + FileWriter() {} + FileWriter(const FileWriter&) = delete; + FileWriter& operator=(const FileWriter&) = delete; + + virtual ~FileWriter() {} + + // Write 'data' to the beginning of the file. This is required to overwrite + // fixed size headers which contain the number of points once we actually know + // how many points there are. + virtual bool WriteHeader(const char* data, size_t len) = 0; + + virtual bool Write(const char* data, size_t len) = 0; + virtual bool Close() = 0; +}; + +// An Implementation of file using std::ofstream. +class StreamFileWriter : public FileWriter { + public: + virtual ~StreamFileWriter() override; + + StreamFileWriter(const string& filename); + + bool Write(const char* data, size_t len) override; + bool WriteHeader(const char* data, size_t len) override; + bool Close() override; + + private: + std::ofstream out_; +}; + +using FileWriterFactory = + std::function(const string& filename)>; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_FILE_WRITER_H_ diff --git a/cartographer/io/pcd_writing_points_processor.cc b/cartographer/io/pcd_writing_points_processor.cc index a56f7c2..27ddefd 100644 --- a/cartographer/io/pcd_writing_points_processor.cc +++ b/cartographer/io/pcd_writing_points_processor.cc @@ -17,6 +17,8 @@ #include "cartographer/io/pcd_writing_points_processor.h" #include +#include +#include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" @@ -31,69 +33,69 @@ namespace { // Writes the PCD header claiming 'num_points' will follow it into // 'output_file'. void WriteBinaryPcdHeader(const bool has_color, const int64 num_points, - std::ofstream* const stream) { + FileWriter* const file_writer) { string color_header_field = !has_color ? "" : " rgb"; string color_header_type = !has_color ? "" : " U"; string color_header_size = !has_color ? "" : " 4"; string color_header_count = !has_color ? "" : " 1"; - (*stream) << "# generated by Cartographer\n" - << "VERSION .7\n" - << "FIELDS x y z" << color_header_field << "\n" - << "SIZE 4 4 4" << color_header_size << "\n" - << "TYPE F F F" << color_header_type << "\n" - << "COUNT 1 1 1" << color_header_count << "\n" - << "WIDTH " << std::setw(15) << std::setfill('0') << num_points - << "\n" - << "HEIGHT 1\n" - << "VIEWPOINT 0 0 0 1 0 0 0\n" - << "POINTS " << std::setw(15) << std::setfill('0') << num_points - << "\n" - << "DATA binary\n"; + std::ostringstream stream; + stream << "# generated by Cartographer\n" + << "VERSION .7\n" + << "FIELDS x y z" << color_header_field << "\n" + << "SIZE 4 4 4" << color_header_size << "\n" + << "TYPE F F F" << color_header_type << "\n" + << "COUNT 1 1 1" << color_header_count << "\n" + << "WIDTH " << std::setw(15) << std::setfill('0') << num_points << "\n" + << "HEIGHT 1\n" + << "VIEWPOINT 0 0 0 1 0 0 0\n" + << "POINTS " << std::setw(15) << std::setfill('0') << num_points + << "\n" + << "DATA binary\n"; + const string out = stream.str(); + file_writer->WriteHeader(out.data(), out.size()); } void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point, - std::ofstream* const stream) { + FileWriter* const file_writer) { char buffer[12]; memcpy(buffer, &point[0], sizeof(float)); memcpy(buffer + 4, &point[1], sizeof(float)); memcpy(buffer + 8, &point[2], sizeof(float)); - for (int i = 0; i < 12; ++i) { - stream->put(buffer[i]); - } + CHECK(file_writer->Write(buffer, 12)); } -void WriteBinaryPcdPointColor(const Color& color, std::ostream* const stream) { - // Pack the color as uint32 little-endian - stream->put(color[2]); - stream->put(color[1]); - stream->put(color[0]); - stream->put(0); +void WriteBinaryPcdPointColor(const Color& color, + FileWriter* const file_writer) { + char buffer[4]; + buffer[0] = color[2]; + buffer[1] = color[1]; + buffer[2] = color[0]; + buffer[3] = 0; + CHECK(file_writer->Write(buffer, 4)); } } // namespace std::unique_ptr PcdWritingPointsProcessor::FromDictionary( + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( - dictionary->GetString("filename"), next); + file_writer_factory(dictionary->GetString("filename")), next); } PcdWritingPointsProcessor::PcdWritingPointsProcessor( - const string& filename, PointsProcessor* const next) + std::unique_ptr file_writer, PointsProcessor* const next) : next_(next), num_points_(0), has_colors_(false), - file_(filename, std::ios_base::out | std::ios_base::binary) {} + file_writer_(std::move(file_writer)) {} PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush() { - file_.flush(); - file_.seekp(0); - - WriteBinaryPcdHeader(has_colors_, num_points_, &file_); - file_.flush(); + WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get()); + CHECK(file_writer_->Close()); switch (next_->Flush()) { case FlushResult::kFinished: @@ -114,12 +116,12 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr batch) { if (num_points_ == 0) { has_colors_ = !batch->colors.empty(); - WriteBinaryPcdHeader(has_colors_, 0, &file_); + WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get()); } for (size_t i = 0; i < batch->points.size(); ++i) { - WriteBinaryPcdPointCoordinate(batch->points[i], &file_); + WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get()); if (!batch->colors.empty()) { - WriteBinaryPcdPointColor(batch->colors[i], &file_); + WriteBinaryPcdPointColor(batch->colors[i], file_writer_.get()); } ++num_points_; } diff --git a/cartographer/io/pcd_writing_points_processor.h b/cartographer/io/pcd_writing_points_processor.h index 5a6debf..8ca3987 100644 --- a/cartographer/io/pcd_writing_points_processor.h +++ b/cartographer/io/pcd_writing_points_processor.h @@ -17,6 +17,7 @@ #include #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" namespace cartographer { @@ -26,9 +27,11 @@ namespace io { class PcdWritingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_pcd"; - PcdWritingPointsProcessor(const string& filename, PointsProcessor* next); + PcdWritingPointsProcessor(std::unique_ptr file_writer, + PointsProcessor* next); static std::unique_ptr FromDictionary( + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~PcdWritingPointsProcessor() override {} @@ -45,7 +48,7 @@ class PcdWritingPointsProcessor : public PointsProcessor { int64 num_points_; bool has_colors_; - std::ofstream file_; + std::unique_ptr file_writer_; }; } // namespace io diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc index 17053eb..aa4b3f4 100644 --- a/cartographer/io/ply_writing_points_processor.cc +++ b/cartographer/io/ply_writing_points_processor.cc @@ -17,6 +17,8 @@ #include "cartographer/io/ply_writing_points_processor.h" #include +#include +#include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" @@ -31,63 +33,60 @@ namespace { // Writes the PLY header claiming 'num_points' will follow it into // 'output_file'. void WriteBinaryPlyHeader(const bool has_color, const int64 num_points, - std::ofstream* const stream) { + FileWriter* const file_writer) { string color_header = !has_color ? "" : "property uchar red\n" "property uchar green\n" "property uchar blue\n"; - - (*stream) << "ply\n" - << "format binary_little_endian 1.0\n" - << "comment generated by Cartographer\n" - << "element vertex " << std::setw(15) << std::setfill('0') - << num_points << "\n" - << "property float x\n" - << "property float y\n" - << "property float z\n" - << color_header << "end_header\n"; + std::ostringstream stream; + stream << "ply\n" + << "format binary_little_endian 1.0\n" + << "comment generated by Cartographer\n" + << "element vertex " << std::setw(15) << std::setfill('0') + << num_points << "\n" + << "property float x\n" + << "property float y\n" + << "property float z\n" + << color_header << "end_header\n"; + const string out = stream.str(); + CHECK(file_writer->WriteHeader(out.data(), out.size())); } void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point, - std::ofstream* const stream) { + FileWriter* const file_writer) { char buffer[12]; memcpy(buffer, &point[0], sizeof(float)); memcpy(buffer + 4, &point[1], sizeof(float)); memcpy(buffer + 8, &point[2], sizeof(float)); - for (int i = 0; i < 12; ++i) { - stream->put(buffer[i]); - } + CHECK(file_writer->Write(buffer, 12)); } -void WriteBinaryPlyPointColor(const Color& color, std::ostream* const stream) { - stream->put(color[0]); - stream->put(color[1]); - stream->put(color[2]); +void WriteBinaryPlyPointColor(const Color& color, + FileWriter* const file_writer) { + CHECK(file_writer->Write(reinterpret_cast(color.data()), + color.size())); } } // namespace std::unique_ptr PlyWritingPointsProcessor::FromDictionary( + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( - dictionary->GetString("filename"), next); + file_writer_factory(dictionary->GetString("filename")), next); } PlyWritingPointsProcessor::PlyWritingPointsProcessor( - const string& filename, PointsProcessor* const next) + std::unique_ptr file_writer, PointsProcessor* const next) : next_(next), num_points_(0), has_colors_(false), - file_(filename, std::ios_base::out | std::ios_base::binary) {} + file_(std::move(file_writer)) {} PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() { - file_.flush(); - file_.seekp(0); - - WriteBinaryPlyHeader(has_colors_, num_points_, &file_); - file_.close(); - CHECK(file_) << "Writing PLY file failed."; + WriteBinaryPlyHeader(has_colors_, num_points_, file_.get()); + CHECK(file_->Close()) << "Closing PLY file_writer failed."; switch (next_->Flush()) { case FlushResult::kFinished: @@ -108,12 +107,12 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr batch) { if (num_points_ == 0) { has_colors_ = !batch->colors.empty(); - WriteBinaryPlyHeader(has_colors_, 0, &file_); + WriteBinaryPlyHeader(has_colors_, 0, file_.get()); } for (size_t i = 0; i < batch->points.size(); ++i) { - WriteBinaryPlyPointCoordinate(batch->points[i], &file_); + WriteBinaryPlyPointCoordinate(batch->points[i], file_.get()); if (!batch->colors.empty()) { - WriteBinaryPlyPointColor(batch->colors[i], &file_); + WriteBinaryPlyPointColor(batch->colors[i], file_.get()); } ++num_points_; } diff --git a/cartographer/io/ply_writing_points_processor.h b/cartographer/io/ply_writing_points_processor.h index 482298e..dab43ee 100644 --- a/cartographer/io/ply_writing_points_processor.h +++ b/cartographer/io/ply_writing_points_processor.h @@ -14,9 +14,8 @@ * limitations under the License. */ -#include - #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" namespace cartographer { @@ -26,9 +25,11 @@ namespace io { class PlyWritingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_ply"; - PlyWritingPointsProcessor(const string& filename, PointsProcessor* next); + PlyWritingPointsProcessor(std::unique_ptr file, + PointsProcessor* next); static std::unique_ptr FromDictionary( + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~PlyWritingPointsProcessor() override {} @@ -45,7 +46,7 @@ class PlyWritingPointsProcessor : public PointsProcessor { int64 num_points_; bool has_colors_; - std::ofstream file_; + std::unique_ptr file_; }; } // namespace io diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index e89d315..bd968f3 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -42,31 +42,50 @@ void RegisterPlainPointsProcessor( }); } +template +void RegisterFileWritingPointsProcessor( + const FileWriterFactory& file_writer_factory, + PointsProcessorPipelineBuilder* const builder) { + builder->Register( + PointsProcessorType::kConfigurationFileActionName, + [&file_writer_factory]( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) -> std::unique_ptr { + return PointsProcessorType::FromDictionary(file_writer_factory, + dictionary, next); + }); +} + void RegisterBuiltInPointsProcessors( const mapping::proto::Trajectory& trajectory, + const FileWriterFactory& file_writer_factory, PointsProcessorPipelineBuilder* builder) { RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); - RegisterPlainPointsProcessor(builder); - RegisterPlainPointsProcessor(builder); - RegisterPlainPointsProcessor(builder); + RegisterFileWritingPointsProcessor( + file_writer_factory, builder); + RegisterFileWritingPointsProcessor( + file_writer_factory, builder); + RegisterFileWritingPointsProcessor( + 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. builder->Register( XRayPointsProcessor::kConfigurationFileActionName, - [&trajectory]( + [&trajectory, &file_writer_factory]( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) -> std::unique_ptr { - return XRayPointsProcessor::FromDictionary(trajectory, dictionary, - next); + return XRayPointsProcessor::FromDictionary( + trajectory, file_writer_factory, dictionary, next); }); } void PointsProcessorPipelineBuilder::Register(const std::string& name, FactoryFunction factory) { - CHECK(factories_.count(name) == 0) << "A points processor with named '" - << name + CHECK(factories_.count(name) == 0) << "A points processor named '" << name << "' has already been registered."; factories_[name] = factory; } diff --git a/cartographer/io/points_processor_pipeline_builder.h b/cartographer/io/points_processor_pipeline_builder.h index 8045884..accd234 100644 --- a/cartographer/io/points_processor_pipeline_builder.h +++ b/cartographer/io/points_processor_pipeline_builder.h @@ -22,6 +22,7 @@ #include #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" #include "cartographer/mapping/proto/trajectory.pb.h" @@ -61,6 +62,7 @@ class PointsProcessorPipelineBuilder { // 'builder'. void RegisterBuiltInPointsProcessors( const mapping::proto::Trajectory& trajectory, + const FileWriterFactory& file_writer_factory, PointsProcessorPipelineBuilder* builder); } // namespace io diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index 59f833b..bbf406a 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -48,8 +48,18 @@ void TakeLogarithm(Eigen::MatrixXf* mat) { } } +cairo_status_t CairoWriteCallback(void* const closure, + const unsigned char* data, + const unsigned int length) { + if (static_cast(closure)->Write( + reinterpret_cast(data), length)) { + return CAIRO_STATUS_SUCCESS; + } + return CAIRO_STATUS_WRITE_ERROR; +} + // Write 'mat' as a pleasing-to-look-at PNG into 'filename' -void WritePng(const string& filename, const Eigen::MatrixXf& mat) { +void WritePng(const Eigen::MatrixXf& mat, FileWriter* const file_writer) { const int stride = cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols()); CHECK_EQ(stride % 4, 0); @@ -76,11 +86,13 @@ void WritePng(const string& filename, const Eigen::MatrixXf& mat) { mat.cols(), mat.rows(), stride), cairo_surface_destroy); CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS); - CHECK_EQ(cairo_surface_write_to_png(surface.get(), filename.c_str()), + CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback, + file_writer), CAIRO_STATUS_SUCCESS); + CHECK(file_writer->Close()); } -void WriteVoxels(const string& filename, const Voxels& voxels) { +void WriteVoxels(const Voxels& voxels, FileWriter* const file_writer) { Eigen::Array3i min(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); @@ -111,7 +123,7 @@ void WriteVoxels(const string& filename, const Voxels& voxels) { ++image(pixel.y(), pixel.x()); } TakeLogarithm(&image); - WritePng(filename, image); + WritePng(image, file_writer); } bool ContainedIn( @@ -138,8 +150,9 @@ void Insert(const PointsBatch& batch, const transform::Rigid3f& transform, XRayPointsProcessor::XRayPointsProcessor( const double voxel_size, const transform::Rigid3f& transform, const std::vector& floors, const string& output_filename, - PointsProcessor* next) + const FileWriterFactory& file_writer_factory, PointsProcessor* const next) : next_(next), + file_writer_factory_(file_writer_factory), floors_(floors), output_filename_(output_filename), transform_(transform) { @@ -150,7 +163,9 @@ XRayPointsProcessor::XRayPointsProcessor( std::unique_ptr XRayPointsProcessor::FromDictionary( const mapping::proto::Trajectory& trajectory, - common::LuaParameterDictionary* dictionary, PointsProcessor* next) { + const FileWriterFactory& file_writer_factory, + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { std::vector floors; if (dictionary->HasKey("separate_floors") && dictionary->GetBool("separate_floors")) { @@ -161,7 +176,7 @@ std::unique_ptr XRayPointsProcessor::FromDictionary( dictionary->GetDouble("voxel_size"), transform::FromDictionary(dictionary->GetDictionary("transform").get()) .cast(), - floors, dictionary->GetString("filename"), next); + floors, dictionary->GetString("filename"), file_writer_factory, next); } void XRayPointsProcessor::Process(std::unique_ptr batch) { @@ -182,10 +197,14 @@ void XRayPointsProcessor::Process(std::unique_ptr batch) { PointsProcessor::FlushResult XRayPointsProcessor::Flush() { if (floors_.empty()) { CHECK_EQ(voxels_.size(), 1); - WriteVoxels(output_filename_ + ".png", voxels_[0]); + WriteVoxels(voxels_[0], + file_writer_factory_(output_filename_ + ".png").get()); } else { for (size_t i = 0; i < floors_.size(); ++i) { - WriteVoxels(output_filename_ + std::to_string(i) + ".png", voxels_[i]); + WriteVoxels( + voxels_[i], + file_writer_factory_(output_filename_ + std::to_string(i) + ".png") + .get()); } } diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h index bee8a41..443fe9c 100644 --- a/cartographer/io/xray_points_processor.h +++ b/cartographer/io/xray_points_processor.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_ #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" #include "cartographer/mapping/detect_floors.h" #include "cartographer/mapping/proto/trajectory.pb.h" @@ -34,10 +35,13 @@ class XRayPointsProcessor : public PointsProcessor { "write_xray_image"; XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform, const std::vector& floors, - const string& output_filename, PointsProcessor* next); + const string& output_filename, + const FileWriterFactory& file_writer_factory, + PointsProcessor* next); static std::unique_ptr FromDictionary( const mapping::proto::Trajectory& trajectory, + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~XRayPointsProcessor() override {} @@ -47,6 +51,7 @@ class XRayPointsProcessor : public PointsProcessor { private: PointsProcessor* const next_; + const FileWriterFactory& file_writer_factory_; // If empty, we do not separate into floors. std::vector floors_; diff --git a/cartographer/io/xyz_writing_points_processor.cc b/cartographer/io/xyz_writing_points_processor.cc index 6d197ed..f23f0e4 100644 --- a/cartographer/io/xyz_writing_points_processor.cc +++ b/cartographer/io/xyz_writing_points_processor.cc @@ -10,29 +10,32 @@ namespace io { namespace { -void WriteXyzPoint(const Eigen::Vector3f& point, std::ofstream* output) { - (*output) << point.x() << " " << point.y() << " " << point.z() << "\n"; +void WriteXyzPoint(const Eigen::Vector3f& point, + FileWriter* const file_writer) { + std::ostringstream stream; + stream << std::setprecision(6); + stream << point.x() << " " << point.y() << " " << point.z() << "\n"; + const string out = stream.str(); + CHECK(file_writer->Write(out.data(), out.size())); } } // namespace -XyzWriterPointsProcessor::XyzWriterPointsProcessor(const string& filename, - PointsProcessor* next) - : next_(next), file_(filename, std::ios_base::out | std::ios_base::binary) { - file_ << std::setprecision(6); -} +XyzWriterPointsProcessor::XyzWriterPointsProcessor( + std::unique_ptr file_writer, PointsProcessor* const next) + : next_(next), file_writer_(std::move(file_writer)) {} std::unique_ptr XyzWriterPointsProcessor::FromDictionary( + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( - dictionary->GetString("filename"), next); + file_writer_factory(dictionary->GetString("filename")), next); } PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() { - file_.close(); - CHECK(file_) << "Writing XYZ file failed."; + CHECK(file_writer_->Close()) << "Closing XYZ file failed."; switch (next_->Flush()) { case FlushResult::kFinished: return FlushResult::kFinished; @@ -46,7 +49,7 @@ PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() { void XyzWriterPointsProcessor::Process(std::unique_ptr batch) { for (const Eigen::Vector3f& point : batch->points) { - WriteXyzPoint(point, &file_); + WriteXyzPoint(point, file_writer_.get()); } next_->Process(std::move(batch)); } diff --git a/cartographer/io/xyz_writing_points_processor.h b/cartographer/io/xyz_writing_points_processor.h index 5443d30..c0e0c64 100644 --- a/cartographer/io/xyz_writing_points_processor.h +++ b/cartographer/io/xyz_writing_points_processor.h @@ -18,9 +18,11 @@ #define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ #include +#include #include #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" namespace cartographer { @@ -31,9 +33,10 @@ class XyzWriterPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_xyz"; - XyzWriterPointsProcessor(const string& filename, PointsProcessor* next); + XyzWriterPointsProcessor(std::unique_ptr, PointsProcessor* next); static std::unique_ptr FromDictionary( + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~XyzWriterPointsProcessor() override {} @@ -46,7 +49,7 @@ class XyzWriterPointsProcessor : public PointsProcessor { private: PointsProcessor* const next_; - std::ofstream file_; + std::unique_ptr file_writer_; }; } // namespace io