12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Adds a FileWriter abstraction (#182)

master
Holger Rapp 2017-01-17 12:29:11 +01:00 committed by GitHub
parent 1f27268664
commit 99f79e3f69
12 changed files with 280 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
cartographer/io/file_writer.cc Normal file
View File

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

68
cartographer/io/file_writer.h Normal file
View File

@ -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 <fstream>
#include <functional>
#include <memory>
#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<std::unique_ptr<FileWriter>(const string& filename)>;
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_FILE_WRITER_H_

72
cartographer/io/pcd_writing_points_processor.cc
View File

@ -17,6 +17,8 @@
#include "cartographer/io/pcd_writing_points_processor.h"
#include <iomanip>
#include <sstream>
#include <string>
#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>
PcdWritingPointsProcessor::FromDictionary(
const FileWriterFactory& file_writer_factory,
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<PcdWritingPointsProcessor>(
dictionary->GetString("filename"), next);
file_writer_factory(dictionary->GetString("filename")), next);
}
PcdWritingPointsProcessor::PcdWritingPointsProcessor(
const string& filename, PointsProcessor* const next)
std::unique_ptr<FileWriter> 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<PointsBatch> 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_;
}

7
cartographer/io/pcd_writing_points_processor.h
View File

@ -17,6 +17,7 @@
#include <fstream>
#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<FileWriter> file_writer,
PointsProcessor* next);
static std::unique_ptr<PcdWritingPointsProcessor> 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<FileWriter> file_writer_;
};
} // namespace io

61
cartographer/io/ply_writing_points_processor.cc
View File

@ -17,6 +17,8 @@
#include "cartographer/io/ply_writing_points_processor.h"
#include <iomanip>
#include <sstream>
#include <string>
#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<const char*>(color.data()),
color.size()));
}
} // namespace
std::unique_ptr<PlyWritingPointsProcessor>
PlyWritingPointsProcessor::FromDictionary(
const FileWriterFactory& file_writer_factory,
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<PlyWritingPointsProcessor>(
dictionary->GetString("filename"), next);
file_writer_factory(dictionary->GetString("filename")), next);
}
PlyWritingPointsProcessor::PlyWritingPointsProcessor(
const string& filename, PointsProcessor* const next)
std::unique_ptr<FileWriter> 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<PointsBatch> 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_;
}

9
cartographer/io/ply_writing_points_processor.h
View File

@ -14,9 +14,8 @@
* limitations under the License.
*/
#include <fstream>
#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<FileWriter> file,
PointsProcessor* next);
static std::unique_ptr<PlyWritingPointsProcessor> 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<FileWriter> file_;
};
} // namespace io

35
cartographer/io/points_processor_pipeline_builder.cc
View File

@ -42,31 +42,50 @@ void RegisterPlainPointsProcessor(
});
}
template <typename PointsProcessorType>
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<PointsProcessor> {
return PointsProcessorType::FromDictionary(file_writer_factory,
dictionary, next);
});
}
void RegisterBuiltInPointsProcessors(
const mapping::proto::Trajectory& trajectory,
const FileWriterFactory& file_writer_factory,
PointsProcessorPipelineBuilder* builder) {
RegisterPlainPointsProcessor<CountingPointsProcessor>(builder);
RegisterPlainPointsProcessor<FixedRatioSamplingPointsProcessor>(builder);
RegisterPlainPointsProcessor<MinMaxRangeFiteringPointsProcessor>(builder);
RegisterPlainPointsProcessor<OutlierRemovingPointsProcessor>(builder);
RegisterPlainPointsProcessor<PcdWritingPointsProcessor>(builder);
RegisterPlainPointsProcessor<PlyWritingPointsProcessor>(builder);
RegisterPlainPointsProcessor<XyzWriterPointsProcessor>(builder);
RegisterFileWritingPointsProcessor<PcdWritingPointsProcessor>(
file_writer_factory, builder);
RegisterFileWritingPointsProcessor<PlyWritingPointsProcessor>(
file_writer_factory, builder);
RegisterFileWritingPointsProcessor<XyzWriterPointsProcessor>(
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<PointsProcessor> {
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;
}

2
cartographer/io/points_processor_pipeline_builder.h
View File

@ -22,6 +22,7 @@
#include <vector>
#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

37
cartographer/io/xray_points_processor.cc
View File

@ -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<FileWriter*>(closure)->Write(
reinterpret_cast<const char*>(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<int>::max(),
std::numeric_limits<int>::max(),
std::numeric_limits<int>::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<mapping::Floor>& 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> 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<mapping::Floor> floors;
if (dictionary->HasKey("separate_floors") &&
dictionary->GetBool("separate_floors")) {
@ -161,7 +176,7 @@ std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
dictionary->GetDouble("voxel_size"),
transform::FromDictionary(dictionary->GetDictionary("transform").get())
.cast<float>(),
floors, dictionary->GetString("filename"), next);
floors, dictionary->GetString("filename"), file_writer_factory, next);
}
void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
@ -182,10 +197,14 @@ void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> 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());
}
}

7
cartographer/io/xray_points_processor.h
View File

@ -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<mapping::Floor>& floors,
const string& output_filename, PointsProcessor* next);
const string& output_filename,
const FileWriterFactory& file_writer_factory,
PointsProcessor* next);
static std::unique_ptr<XRayPointsProcessor> 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<mapping::Floor> floors_;

25
cartographer/io/xyz_writing_points_processor.cc
View File

@ -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<FileWriter> file_writer, PointsProcessor* const next)
: next_(next), file_writer_(std::move(file_writer)) {}
std::unique_ptr<XyzWriterPointsProcessor>
XyzWriterPointsProcessor::FromDictionary(
const FileWriterFactory& file_writer_factory,
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<XyzWriterPointsProcessor>(
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<PointsBatch> batch) {
for (const Eigen::Vector3f& point : batch->points) {
WriteXyzPoint(point, &file_);
WriteXyzPoint(point, file_writer_.get());
}
next_->Process(std::move(batch));
}

7
cartographer/io/xyz_writing_points_processor.h
View File

@ -18,9 +18,11 @@
#define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_
#include <fstream>
#include <memory>
#include <string>
#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<FileWriter>, PointsProcessor* next);
static std::unique_ptr<XyzWriterPointsProcessor> 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<FileWriter> file_writer_;
};
} // namespace io