Adds a FileWriter abstraction (#182)
parent
1f27268664
commit
99f79e3f69
|
@ -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
|
|
@ -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_
|
|
@ -17,6 +17,8 @@
|
||||||
#include "cartographer/io/pcd_writing_points_processor.h"
|
#include "cartographer/io/pcd_writing_points_processor.h"
|
||||||
|
|
||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
|
#include <sstream>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
|
@ -31,69 +33,69 @@ namespace {
|
||||||
// Writes the PCD header claiming 'num_points' will follow it into
|
// Writes the PCD header claiming 'num_points' will follow it into
|
||||||
// 'output_file'.
|
// 'output_file'.
|
||||||
void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
|
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_field = !has_color ? "" : " rgb";
|
||||||
string color_header_type = !has_color ? "" : " U";
|
string color_header_type = !has_color ? "" : " U";
|
||||||
string color_header_size = !has_color ? "" : " 4";
|
string color_header_size = !has_color ? "" : " 4";
|
||||||
string color_header_count = !has_color ? "" : " 1";
|
string color_header_count = !has_color ? "" : " 1";
|
||||||
|
|
||||||
(*stream) << "# generated by Cartographer\n"
|
std::ostringstream stream;
|
||||||
|
stream << "# generated by Cartographer\n"
|
||||||
<< "VERSION .7\n"
|
<< "VERSION .7\n"
|
||||||
<< "FIELDS x y z" << color_header_field << "\n"
|
<< "FIELDS x y z" << color_header_field << "\n"
|
||||||
<< "SIZE 4 4 4" << color_header_size << "\n"
|
<< "SIZE 4 4 4" << color_header_size << "\n"
|
||||||
<< "TYPE F F F" << color_header_type << "\n"
|
<< "TYPE F F F" << color_header_type << "\n"
|
||||||
<< "COUNT 1 1 1" << color_header_count << "\n"
|
<< "COUNT 1 1 1" << color_header_count << "\n"
|
||||||
<< "WIDTH " << std::setw(15) << std::setfill('0') << num_points
|
<< "WIDTH " << std::setw(15) << std::setfill('0') << num_points << "\n"
|
||||||
<< "\n"
|
|
||||||
<< "HEIGHT 1\n"
|
<< "HEIGHT 1\n"
|
||||||
<< "VIEWPOINT 0 0 0 1 0 0 0\n"
|
<< "VIEWPOINT 0 0 0 1 0 0 0\n"
|
||||||
<< "POINTS " << std::setw(15) << std::setfill('0') << num_points
|
<< "POINTS " << std::setw(15) << std::setfill('0') << num_points
|
||||||
<< "\n"
|
<< "\n"
|
||||||
<< "DATA binary\n";
|
<< "DATA binary\n";
|
||||||
|
const string out = stream.str();
|
||||||
|
file_writer->WriteHeader(out.data(), out.size());
|
||||||
}
|
}
|
||||||
|
|
||||||
void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
|
void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
|
||||||
std::ofstream* const stream) {
|
FileWriter* const file_writer) {
|
||||||
char buffer[12];
|
char buffer[12];
|
||||||
memcpy(buffer, &point[0], sizeof(float));
|
memcpy(buffer, &point[0], sizeof(float));
|
||||||
memcpy(buffer + 4, &point[1], sizeof(float));
|
memcpy(buffer + 4, &point[1], sizeof(float));
|
||||||
memcpy(buffer + 8, &point[2], sizeof(float));
|
memcpy(buffer + 8, &point[2], sizeof(float));
|
||||||
for (int i = 0; i < 12; ++i) {
|
CHECK(file_writer->Write(buffer, 12));
|
||||||
stream->put(buffer[i]);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void WriteBinaryPcdPointColor(const Color& color, std::ostream* const stream) {
|
void WriteBinaryPcdPointColor(const Color& color,
|
||||||
// Pack the color as uint32 little-endian
|
FileWriter* const file_writer) {
|
||||||
stream->put(color[2]);
|
char buffer[4];
|
||||||
stream->put(color[1]);
|
buffer[0] = color[2];
|
||||||
stream->put(color[0]);
|
buffer[1] = color[1];
|
||||||
stream->put(0);
|
buffer[2] = color[0];
|
||||||
|
buffer[3] = 0;
|
||||||
|
CHECK(file_writer->Write(buffer, 4));
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
std::unique_ptr<PcdWritingPointsProcessor>
|
std::unique_ptr<PcdWritingPointsProcessor>
|
||||||
PcdWritingPointsProcessor::FromDictionary(
|
PcdWritingPointsProcessor::FromDictionary(
|
||||||
|
const FileWriterFactory& file_writer_factory,
|
||||||
common::LuaParameterDictionary* const dictionary,
|
common::LuaParameterDictionary* const dictionary,
|
||||||
PointsProcessor* const next) {
|
PointsProcessor* const next) {
|
||||||
return common::make_unique<PcdWritingPointsProcessor>(
|
return common::make_unique<PcdWritingPointsProcessor>(
|
||||||
dictionary->GetString("filename"), next);
|
file_writer_factory(dictionary->GetString("filename")), next);
|
||||||
}
|
}
|
||||||
|
|
||||||
PcdWritingPointsProcessor::PcdWritingPointsProcessor(
|
PcdWritingPointsProcessor::PcdWritingPointsProcessor(
|
||||||
const string& filename, PointsProcessor* const next)
|
std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
|
||||||
: next_(next),
|
: next_(next),
|
||||||
num_points_(0),
|
num_points_(0),
|
||||||
has_colors_(false),
|
has_colors_(false),
|
||||||
file_(filename, std::ios_base::out | std::ios_base::binary) {}
|
file_writer_(std::move(file_writer)) {}
|
||||||
|
|
||||||
PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush() {
|
PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush() {
|
||||||
file_.flush();
|
WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get());
|
||||||
file_.seekp(0);
|
CHECK(file_writer_->Close());
|
||||||
|
|
||||||
WriteBinaryPcdHeader(has_colors_, num_points_, &file_);
|
|
||||||
file_.flush();
|
|
||||||
|
|
||||||
switch (next_->Flush()) {
|
switch (next_->Flush()) {
|
||||||
case FlushResult::kFinished:
|
case FlushResult::kFinished:
|
||||||
|
@ -114,12 +116,12 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
||||||
|
|
||||||
if (num_points_ == 0) {
|
if (num_points_ == 0) {
|
||||||
has_colors_ = !batch->colors.empty();
|
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) {
|
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()) {
|
if (!batch->colors.empty()) {
|
||||||
WriteBinaryPcdPointColor(batch->colors[i], &file_);
|
WriteBinaryPcdPointColor(batch->colors[i], file_writer_.get());
|
||||||
}
|
}
|
||||||
++num_points_;
|
++num_points_;
|
||||||
}
|
}
|
||||||
|
|
|
@ -17,6 +17,7 @@
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
|
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
|
#include "cartographer/io/file_writer.h"
|
||||||
#include "cartographer/io/points_processor.h"
|
#include "cartographer/io/points_processor.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
|
@ -26,9 +27,11 @@ namespace io {
|
||||||
class PcdWritingPointsProcessor : public PointsProcessor {
|
class PcdWritingPointsProcessor : public PointsProcessor {
|
||||||
public:
|
public:
|
||||||
constexpr static const char* kConfigurationFileActionName = "write_pcd";
|
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(
|
static std::unique_ptr<PcdWritingPointsProcessor> FromDictionary(
|
||||||
|
const FileWriterFactory& file_writer_factory,
|
||||||
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
||||||
|
|
||||||
~PcdWritingPointsProcessor() override {}
|
~PcdWritingPointsProcessor() override {}
|
||||||
|
@ -45,7 +48,7 @@ class PcdWritingPointsProcessor : public PointsProcessor {
|
||||||
|
|
||||||
int64 num_points_;
|
int64 num_points_;
|
||||||
bool has_colors_;
|
bool has_colors_;
|
||||||
std::ofstream file_;
|
std::unique_ptr<FileWriter> file_writer_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace io
|
} // namespace io
|
||||||
|
|
|
@ -17,6 +17,8 @@
|
||||||
#include "cartographer/io/ply_writing_points_processor.h"
|
#include "cartographer/io/ply_writing_points_processor.h"
|
||||||
|
|
||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
|
#include <sstream>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
|
@ -31,12 +33,12 @@ namespace {
|
||||||
// Writes the PLY header claiming 'num_points' will follow it into
|
// Writes the PLY header claiming 'num_points' will follow it into
|
||||||
// 'output_file'.
|
// 'output_file'.
|
||||||
void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
|
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"
|
string color_header = !has_color ? "" : "property uchar red\n"
|
||||||
"property uchar green\n"
|
"property uchar green\n"
|
||||||
"property uchar blue\n";
|
"property uchar blue\n";
|
||||||
|
std::ostringstream stream;
|
||||||
(*stream) << "ply\n"
|
stream << "ply\n"
|
||||||
<< "format binary_little_endian 1.0\n"
|
<< "format binary_little_endian 1.0\n"
|
||||||
<< "comment generated by Cartographer\n"
|
<< "comment generated by Cartographer\n"
|
||||||
<< "element vertex " << std::setw(15) << std::setfill('0')
|
<< "element vertex " << std::setw(15) << std::setfill('0')
|
||||||
|
@ -45,49 +47,46 @@ void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
|
||||||
<< "property float y\n"
|
<< "property float y\n"
|
||||||
<< "property float z\n"
|
<< "property float z\n"
|
||||||
<< color_header << "end_header\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,
|
void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
|
||||||
std::ofstream* const stream) {
|
FileWriter* const file_writer) {
|
||||||
char buffer[12];
|
char buffer[12];
|
||||||
memcpy(buffer, &point[0], sizeof(float));
|
memcpy(buffer, &point[0], sizeof(float));
|
||||||
memcpy(buffer + 4, &point[1], sizeof(float));
|
memcpy(buffer + 4, &point[1], sizeof(float));
|
||||||
memcpy(buffer + 8, &point[2], sizeof(float));
|
memcpy(buffer + 8, &point[2], sizeof(float));
|
||||||
for (int i = 0; i < 12; ++i) {
|
CHECK(file_writer->Write(buffer, 12));
|
||||||
stream->put(buffer[i]);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void WriteBinaryPlyPointColor(const Color& color, std::ostream* const stream) {
|
void WriteBinaryPlyPointColor(const Color& color,
|
||||||
stream->put(color[0]);
|
FileWriter* const file_writer) {
|
||||||
stream->put(color[1]);
|
CHECK(file_writer->Write(reinterpret_cast<const char*>(color.data()),
|
||||||
stream->put(color[2]);
|
color.size()));
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
std::unique_ptr<PlyWritingPointsProcessor>
|
std::unique_ptr<PlyWritingPointsProcessor>
|
||||||
PlyWritingPointsProcessor::FromDictionary(
|
PlyWritingPointsProcessor::FromDictionary(
|
||||||
|
const FileWriterFactory& file_writer_factory,
|
||||||
common::LuaParameterDictionary* const dictionary,
|
common::LuaParameterDictionary* const dictionary,
|
||||||
PointsProcessor* const next) {
|
PointsProcessor* const next) {
|
||||||
return common::make_unique<PlyWritingPointsProcessor>(
|
return common::make_unique<PlyWritingPointsProcessor>(
|
||||||
dictionary->GetString("filename"), next);
|
file_writer_factory(dictionary->GetString("filename")), next);
|
||||||
}
|
}
|
||||||
|
|
||||||
PlyWritingPointsProcessor::PlyWritingPointsProcessor(
|
PlyWritingPointsProcessor::PlyWritingPointsProcessor(
|
||||||
const string& filename, PointsProcessor* const next)
|
std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
|
||||||
: next_(next),
|
: next_(next),
|
||||||
num_points_(0),
|
num_points_(0),
|
||||||
has_colors_(false),
|
has_colors_(false),
|
||||||
file_(filename, std::ios_base::out | std::ios_base::binary) {}
|
file_(std::move(file_writer)) {}
|
||||||
|
|
||||||
PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() {
|
PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() {
|
||||||
file_.flush();
|
WriteBinaryPlyHeader(has_colors_, num_points_, file_.get());
|
||||||
file_.seekp(0);
|
CHECK(file_->Close()) << "Closing PLY file_writer failed.";
|
||||||
|
|
||||||
WriteBinaryPlyHeader(has_colors_, num_points_, &file_);
|
|
||||||
file_.close();
|
|
||||||
CHECK(file_) << "Writing PLY file failed.";
|
|
||||||
|
|
||||||
switch (next_->Flush()) {
|
switch (next_->Flush()) {
|
||||||
case FlushResult::kFinished:
|
case FlushResult::kFinished:
|
||||||
|
@ -108,12 +107,12 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
||||||
|
|
||||||
if (num_points_ == 0) {
|
if (num_points_ == 0) {
|
||||||
has_colors_ = !batch->colors.empty();
|
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) {
|
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()) {
|
if (!batch->colors.empty()) {
|
||||||
WriteBinaryPlyPointColor(batch->colors[i], &file_);
|
WriteBinaryPlyPointColor(batch->colors[i], file_.get());
|
||||||
}
|
}
|
||||||
++num_points_;
|
++num_points_;
|
||||||
}
|
}
|
||||||
|
|
|
@ -14,9 +14,8 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <fstream>
|
|
||||||
|
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
|
#include "cartographer/io/file_writer.h"
|
||||||
#include "cartographer/io/points_processor.h"
|
#include "cartographer/io/points_processor.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
|
@ -26,9 +25,11 @@ namespace io {
|
||||||
class PlyWritingPointsProcessor : public PointsProcessor {
|
class PlyWritingPointsProcessor : public PointsProcessor {
|
||||||
public:
|
public:
|
||||||
constexpr static const char* kConfigurationFileActionName = "write_ply";
|
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(
|
static std::unique_ptr<PlyWritingPointsProcessor> FromDictionary(
|
||||||
|
const FileWriterFactory& file_writer_factory,
|
||||||
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
||||||
|
|
||||||
~PlyWritingPointsProcessor() override {}
|
~PlyWritingPointsProcessor() override {}
|
||||||
|
@ -45,7 +46,7 @@ class PlyWritingPointsProcessor : public PointsProcessor {
|
||||||
|
|
||||||
int64 num_points_;
|
int64 num_points_;
|
||||||
bool has_colors_;
|
bool has_colors_;
|
||||||
std::ofstream file_;
|
std::unique_ptr<FileWriter> file_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace io
|
} // namespace io
|
||||||
|
|
|
@ -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(
|
void RegisterBuiltInPointsProcessors(
|
||||||
const mapping::proto::Trajectory& trajectory,
|
const mapping::proto::Trajectory& trajectory,
|
||||||
|
const FileWriterFactory& file_writer_factory,
|
||||||
PointsProcessorPipelineBuilder* builder) {
|
PointsProcessorPipelineBuilder* builder) {
|
||||||
RegisterPlainPointsProcessor<CountingPointsProcessor>(builder);
|
RegisterPlainPointsProcessor<CountingPointsProcessor>(builder);
|
||||||
RegisterPlainPointsProcessor<FixedRatioSamplingPointsProcessor>(builder);
|
RegisterPlainPointsProcessor<FixedRatioSamplingPointsProcessor>(builder);
|
||||||
RegisterPlainPointsProcessor<MinMaxRangeFiteringPointsProcessor>(builder);
|
RegisterPlainPointsProcessor<MinMaxRangeFiteringPointsProcessor>(builder);
|
||||||
RegisterPlainPointsProcessor<OutlierRemovingPointsProcessor>(builder);
|
RegisterPlainPointsProcessor<OutlierRemovingPointsProcessor>(builder);
|
||||||
RegisterPlainPointsProcessor<PcdWritingPointsProcessor>(builder);
|
RegisterFileWritingPointsProcessor<PcdWritingPointsProcessor>(
|
||||||
RegisterPlainPointsProcessor<PlyWritingPointsProcessor>(builder);
|
file_writer_factory, builder);
|
||||||
RegisterPlainPointsProcessor<XyzWriterPointsProcessor>(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(
|
builder->Register(
|
||||||
XRayPointsProcessor::kConfigurationFileActionName,
|
XRayPointsProcessor::kConfigurationFileActionName,
|
||||||
[&trajectory](
|
[&trajectory, &file_writer_factory](
|
||||||
common::LuaParameterDictionary* const dictionary,
|
common::LuaParameterDictionary* const dictionary,
|
||||||
PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
|
PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
|
||||||
return XRayPointsProcessor::FromDictionary(trajectory, dictionary,
|
return XRayPointsProcessor::FromDictionary(
|
||||||
next);
|
trajectory, file_writer_factory, dictionary, next);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
void PointsProcessorPipelineBuilder::Register(const std::string& name,
|
void PointsProcessorPipelineBuilder::Register(const std::string& name,
|
||||||
FactoryFunction factory) {
|
FactoryFunction factory) {
|
||||||
CHECK(factories_.count(name) == 0) << "A points processor with named '"
|
CHECK(factories_.count(name) == 0) << "A points processor named '" << name
|
||||||
<< name
|
|
||||||
<< "' has already been registered.";
|
<< "' has already been registered.";
|
||||||
factories_[name] = factory;
|
factories_[name] = factory;
|
||||||
}
|
}
|
||||||
|
|
|
@ -22,6 +22,7 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
|
#include "cartographer/io/file_writer.h"
|
||||||
#include "cartographer/io/points_processor.h"
|
#include "cartographer/io/points_processor.h"
|
||||||
#include "cartographer/mapping/proto/trajectory.pb.h"
|
#include "cartographer/mapping/proto/trajectory.pb.h"
|
||||||
|
|
||||||
|
@ -61,6 +62,7 @@ class PointsProcessorPipelineBuilder {
|
||||||
// 'builder'.
|
// 'builder'.
|
||||||
void RegisterBuiltInPointsProcessors(
|
void RegisterBuiltInPointsProcessors(
|
||||||
const mapping::proto::Trajectory& trajectory,
|
const mapping::proto::Trajectory& trajectory,
|
||||||
|
const FileWriterFactory& file_writer_factory,
|
||||||
PointsProcessorPipelineBuilder* builder);
|
PointsProcessorPipelineBuilder* builder);
|
||||||
|
|
||||||
} // namespace io
|
} // namespace io
|
||||||
|
|
|
@ -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'
|
// 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 =
|
const int stride =
|
||||||
cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols());
|
cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols());
|
||||||
CHECK_EQ(stride % 4, 0);
|
CHECK_EQ(stride % 4, 0);
|
||||||
|
@ -76,11 +86,13 @@ void WritePng(const string& filename, const Eigen::MatrixXf& mat) {
|
||||||
mat.cols(), mat.rows(), stride),
|
mat.cols(), mat.rows(), stride),
|
||||||
cairo_surface_destroy);
|
cairo_surface_destroy);
|
||||||
CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS);
|
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);
|
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(),
|
Eigen::Array3i min(std::numeric_limits<int>::max(),
|
||||||
std::numeric_limits<int>::max(),
|
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());
|
++image(pixel.y(), pixel.x());
|
||||||
}
|
}
|
||||||
TakeLogarithm(&image);
|
TakeLogarithm(&image);
|
||||||
WritePng(filename, image);
|
WritePng(image, file_writer);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ContainedIn(
|
bool ContainedIn(
|
||||||
|
@ -138,8 +150,9 @@ void Insert(const PointsBatch& batch, const transform::Rigid3f& transform,
|
||||||
XRayPointsProcessor::XRayPointsProcessor(
|
XRayPointsProcessor::XRayPointsProcessor(
|
||||||
const double voxel_size, const transform::Rigid3f& transform,
|
const double voxel_size, const transform::Rigid3f& transform,
|
||||||
const std::vector<mapping::Floor>& floors, const string& output_filename,
|
const std::vector<mapping::Floor>& floors, const string& output_filename,
|
||||||
PointsProcessor* next)
|
const FileWriterFactory& file_writer_factory, PointsProcessor* const next)
|
||||||
: next_(next),
|
: next_(next),
|
||||||
|
file_writer_factory_(file_writer_factory),
|
||||||
floors_(floors),
|
floors_(floors),
|
||||||
output_filename_(output_filename),
|
output_filename_(output_filename),
|
||||||
transform_(transform) {
|
transform_(transform) {
|
||||||
|
@ -150,7 +163,9 @@ XRayPointsProcessor::XRayPointsProcessor(
|
||||||
|
|
||||||
std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
|
std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
|
||||||
const mapping::proto::Trajectory& trajectory,
|
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;
|
std::vector<mapping::Floor> floors;
|
||||||
if (dictionary->HasKey("separate_floors") &&
|
if (dictionary->HasKey("separate_floors") &&
|
||||||
dictionary->GetBool("separate_floors")) {
|
dictionary->GetBool("separate_floors")) {
|
||||||
|
@ -161,7 +176,7 @@ std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
|
||||||
dictionary->GetDouble("voxel_size"),
|
dictionary->GetDouble("voxel_size"),
|
||||||
transform::FromDictionary(dictionary->GetDictionary("transform").get())
|
transform::FromDictionary(dictionary->GetDictionary("transform").get())
|
||||||
.cast<float>(),
|
.cast<float>(),
|
||||||
floors, dictionary->GetString("filename"), next);
|
floors, dictionary->GetString("filename"), file_writer_factory, next);
|
||||||
}
|
}
|
||||||
|
|
||||||
void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
||||||
|
@ -182,10 +197,14 @@ void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
||||||
PointsProcessor::FlushResult XRayPointsProcessor::Flush() {
|
PointsProcessor::FlushResult XRayPointsProcessor::Flush() {
|
||||||
if (floors_.empty()) {
|
if (floors_.empty()) {
|
||||||
CHECK_EQ(voxels_.size(), 1);
|
CHECK_EQ(voxels_.size(), 1);
|
||||||
WriteVoxels(output_filename_ + ".png", voxels_[0]);
|
WriteVoxels(voxels_[0],
|
||||||
|
file_writer_factory_(output_filename_ + ".png").get());
|
||||||
} else {
|
} else {
|
||||||
for (size_t i = 0; i < floors_.size(); ++i) {
|
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());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -18,6 +18,7 @@
|
||||||
#define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
|
#define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
|
||||||
|
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
|
#include "cartographer/io/file_writer.h"
|
||||||
#include "cartographer/io/points_processor.h"
|
#include "cartographer/io/points_processor.h"
|
||||||
#include "cartographer/mapping/detect_floors.h"
|
#include "cartographer/mapping/detect_floors.h"
|
||||||
#include "cartographer/mapping/proto/trajectory.pb.h"
|
#include "cartographer/mapping/proto/trajectory.pb.h"
|
||||||
|
@ -34,10 +35,13 @@ class XRayPointsProcessor : public PointsProcessor {
|
||||||
"write_xray_image";
|
"write_xray_image";
|
||||||
XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform,
|
XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform,
|
||||||
const std::vector<mapping::Floor>& floors,
|
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(
|
static std::unique_ptr<XRayPointsProcessor> FromDictionary(
|
||||||
const mapping::proto::Trajectory& trajectory,
|
const mapping::proto::Trajectory& trajectory,
|
||||||
|
const FileWriterFactory& file_writer_factory,
|
||||||
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
||||||
|
|
||||||
~XRayPointsProcessor() override {}
|
~XRayPointsProcessor() override {}
|
||||||
|
@ -47,6 +51,7 @@ class XRayPointsProcessor : public PointsProcessor {
|
||||||
|
|
||||||
private:
|
private:
|
||||||
PointsProcessor* const next_;
|
PointsProcessor* const next_;
|
||||||
|
const FileWriterFactory& file_writer_factory_;
|
||||||
|
|
||||||
// If empty, we do not separate into floors.
|
// If empty, we do not separate into floors.
|
||||||
std::vector<mapping::Floor> floors_;
|
std::vector<mapping::Floor> floors_;
|
||||||
|
|
|
@ -10,29 +10,32 @@ namespace io {
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
void WriteXyzPoint(const Eigen::Vector3f& point, std::ofstream* output) {
|
void WriteXyzPoint(const Eigen::Vector3f& point,
|
||||||
(*output) << point.x() << " " << point.y() << " " << point.z() << "\n";
|
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
|
} // namespace
|
||||||
|
|
||||||
XyzWriterPointsProcessor::XyzWriterPointsProcessor(const string& filename,
|
XyzWriterPointsProcessor::XyzWriterPointsProcessor(
|
||||||
PointsProcessor* next)
|
std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
|
||||||
: next_(next), file_(filename, std::ios_base::out | std::ios_base::binary) {
|
: next_(next), file_writer_(std::move(file_writer)) {}
|
||||||
file_ << std::setprecision(6);
|
|
||||||
}
|
|
||||||
|
|
||||||
std::unique_ptr<XyzWriterPointsProcessor>
|
std::unique_ptr<XyzWriterPointsProcessor>
|
||||||
XyzWriterPointsProcessor::FromDictionary(
|
XyzWriterPointsProcessor::FromDictionary(
|
||||||
|
const FileWriterFactory& file_writer_factory,
|
||||||
common::LuaParameterDictionary* const dictionary,
|
common::LuaParameterDictionary* const dictionary,
|
||||||
PointsProcessor* const next) {
|
PointsProcessor* const next) {
|
||||||
return common::make_unique<XyzWriterPointsProcessor>(
|
return common::make_unique<XyzWriterPointsProcessor>(
|
||||||
dictionary->GetString("filename"), next);
|
file_writer_factory(dictionary->GetString("filename")), next);
|
||||||
}
|
}
|
||||||
|
|
||||||
PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() {
|
PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() {
|
||||||
file_.close();
|
CHECK(file_writer_->Close()) << "Closing XYZ file failed.";
|
||||||
CHECK(file_) << "Writing XYZ file failed.";
|
|
||||||
switch (next_->Flush()) {
|
switch (next_->Flush()) {
|
||||||
case FlushResult::kFinished:
|
case FlushResult::kFinished:
|
||||||
return FlushResult::kFinished;
|
return FlushResult::kFinished;
|
||||||
|
@ -46,7 +49,7 @@ PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() {
|
||||||
|
|
||||||
void XyzWriterPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
void XyzWriterPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
||||||
for (const Eigen::Vector3f& point : batch->points) {
|
for (const Eigen::Vector3f& point : batch->points) {
|
||||||
WriteXyzPoint(point, &file_);
|
WriteXyzPoint(point, file_writer_.get());
|
||||||
}
|
}
|
||||||
next_->Process(std::move(batch));
|
next_->Process(std::move(batch));
|
||||||
}
|
}
|
||||||
|
|
|
@ -18,9 +18,11 @@
|
||||||
#define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_
|
#define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_
|
||||||
|
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
|
#include <memory>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
|
#include "cartographer/io/file_writer.h"
|
||||||
#include "cartographer/io/points_processor.h"
|
#include "cartographer/io/points_processor.h"
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
|
@ -31,9 +33,10 @@ class XyzWriterPointsProcessor : public PointsProcessor {
|
||||||
public:
|
public:
|
||||||
constexpr static const char* kConfigurationFileActionName = "write_xyz";
|
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(
|
static std::unique_ptr<XyzWriterPointsProcessor> FromDictionary(
|
||||||
|
const FileWriterFactory& file_writer_factory,
|
||||||
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
||||||
|
|
||||||
~XyzWriterPointsProcessor() override {}
|
~XyzWriterPointsProcessor() override {}
|
||||||
|
@ -46,7 +49,7 @@ class XyzWriterPointsProcessor : public PointsProcessor {
|
||||||
|
|
||||||
private:
|
private:
|
||||||
PointsProcessor* const next_;
|
PointsProcessor* const next_;
|
||||||
std::ofstream file_;
|
std::unique_ptr<FileWriter> file_writer_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace io
|
} // namespace io
|
||||||
|
|
Loading…
Reference in New Issue