59 lines
1.7 KiB
C++
Executable File
59 lines
1.7 KiB
C++
Executable File
#include "cartographer/io/xyz_writing_points_processor.h"
|
|
|
|
#include <iomanip>
|
|
|
|
#include "absl/memory/memory.h"
|
|
#include "glog/logging.h"
|
|
|
|
namespace cartographer {
|
|
namespace io {
|
|
|
|
namespace {
|
|
|
|
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 std::string out = stream.str();
|
|
CHECK(file_writer->Write(out.data(), out.size()));
|
|
}
|
|
|
|
} // namespace
|
|
|
|
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 absl::make_unique<XyzWriterPointsProcessor>(
|
|
file_writer_factory(dictionary->GetString("filename")), next);
|
|
}
|
|
|
|
PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() {
|
|
CHECK(file_writer_->Close()) << "Closing 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 sensor::RangefinderPoint& point : batch->points) {
|
|
WriteXyzPoint(point.position, file_writer_.get());
|
|
}
|
|
next_->Process(std::move(batch));
|
|
}
|
|
|
|
} // namespace io
|
|
} // namespace cartographer
|