00001 #include "cartographer/io/xyz_writing_points_processor.h" 00002 00003 #include <iomanip> 00004 00005 #include "absl/memory/memory.h" 00006 #include "glog/logging.h" 00007 00008 namespace cartographer { 00009 namespace io { 00010 00011 namespace { 00012 00013 void WriteXyzPoint(const Eigen::Vector3f& point, 00014 FileWriter* const file_writer) { 00015 std::ostringstream stream; 00016 stream << std::setprecision(6); 00017 stream << point.x() << " " << point.y() << " " << point.z() << "\n"; 00018 const std::string out = stream.str(); 00019 CHECK(file_writer->Write(out.data(), out.size())); 00020 } 00021 00022 } // namespace 00023 00024 XyzWriterPointsProcessor::XyzWriterPointsProcessor( 00025 std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next) 00026 : next_(next), file_writer_(std::move(file_writer)) {} 00027 00028 std::unique_ptr<XyzWriterPointsProcessor> 00029 XyzWriterPointsProcessor::FromDictionary( 00030 const FileWriterFactory& file_writer_factory, 00031 common::LuaParameterDictionary* const dictionary, 00032 PointsProcessor* const next) { 00033 return absl::make_unique<XyzWriterPointsProcessor>( 00034 file_writer_factory(dictionary->GetString("filename")), next); 00035 } 00036 00037 PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() { 00038 CHECK(file_writer_->Close()) << "Closing XYZ file failed."; 00039 switch (next_->Flush()) { 00040 case FlushResult::kFinished: 00041 return FlushResult::kFinished; 00042 00043 case FlushResult::kRestartStream: 00044 LOG(FATAL) << "XYZ generation must be configured to occur after any " 00045 "stages that require multiple passes."; 00046 } 00047 LOG(FATAL); 00048 } 00049 00050 void XyzWriterPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) { 00051 for (const sensor::RangefinderPoint& point : batch->points) { 00052 WriteXyzPoint(point.position, file_writer_.get()); 00053 } 00054 next_->Process(std::move(batch)); 00055 } 00056 00057 } // namespace io 00058 } // namespace cartographer