Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/io/pcd_writing_points_processor.h"
00018
00019 #include <iomanip>
00020 #include <sstream>
00021 #include <string>
00022
00023 #include "absl/memory/memory.h"
00024 #include "cartographer/common/lua_parameter_dictionary.h"
00025 #include "cartographer/io/points_batch.h"
00026 #include "glog/logging.h"
00027
00028 namespace cartographer {
00029 namespace io {
00030
00031 namespace {
00032
00033
00034
00035 void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
00036 FileWriter* const file_writer) {
00037 std::string color_header_field = !has_color ? "" : " rgb";
00038 std::string color_header_type = !has_color ? "" : " U";
00039 std::string color_header_size = !has_color ? "" : " 4";
00040 std::string color_header_count = !has_color ? "" : " 1";
00041
00042 std::ostringstream stream;
00043 stream << "# generated by Cartographer\n"
00044 << "VERSION .7\n"
00045 << "FIELDS x y z" << color_header_field << "\n"
00046 << "SIZE 4 4 4" << color_header_size << "\n"
00047 << "TYPE F F F" << color_header_type << "\n"
00048 << "COUNT 1 1 1" << color_header_count << "\n"
00049 << "WIDTH " << std::setw(15) << std::setfill('0') << num_points << "\n"
00050 << "HEIGHT 1\n"
00051 << "VIEWPOINT 0 0 0 1 0 0 0\n"
00052 << "POINTS " << std::setw(15) << std::setfill('0') << num_points
00053 << "\n"
00054 << "DATA binary\n";
00055 const std::string out = stream.str();
00056 file_writer->WriteHeader(out.data(), out.size());
00057 }
00058
00059 void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
00060 FileWriter* const file_writer) {
00061 char buffer[12];
00062 memcpy(buffer, &point[0], sizeof(float));
00063 memcpy(buffer + 4, &point[1], sizeof(float));
00064 memcpy(buffer + 8, &point[2], sizeof(float));
00065 CHECK(file_writer->Write(buffer, 12));
00066 }
00067
00068 void WriteBinaryPcdPointColor(const Uint8Color& color,
00069 FileWriter* const file_writer) {
00070 char buffer[4];
00071 buffer[0] = color[2];
00072 buffer[1] = color[1];
00073 buffer[2] = color[0];
00074 buffer[3] = 0;
00075 CHECK(file_writer->Write(buffer, 4));
00076 }
00077
00078 }
00079
00080 std::unique_ptr<PcdWritingPointsProcessor>
00081 PcdWritingPointsProcessor::FromDictionary(
00082 FileWriterFactory file_writer_factory,
00083 common::LuaParameterDictionary* const dictionary,
00084 PointsProcessor* const next) {
00085 return absl::make_unique<PcdWritingPointsProcessor>(
00086 file_writer_factory(dictionary->GetString("filename")), next);
00087 }
00088
00089 PcdWritingPointsProcessor::PcdWritingPointsProcessor(
00090 std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
00091 : next_(next),
00092 num_points_(0),
00093 has_colors_(false),
00094 file_writer_(std::move(file_writer)) {}
00095
00096 PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush() {
00097 WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get());
00098 CHECK(file_writer_->Close());
00099
00100 switch (next_->Flush()) {
00101 case FlushResult::kFinished:
00102 return FlushResult::kFinished;
00103
00104 case FlushResult::kRestartStream:
00105 LOG(FATAL) << "PCD generation must be configured to occur after any "
00106 "stages that require multiple passes.";
00107 }
00108 LOG(FATAL);
00109 }
00110
00111 void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
00112 if (batch->points.empty()) {
00113 next_->Process(std::move(batch));
00114 return;
00115 }
00116
00117 if (num_points_ == 0) {
00118 has_colors_ = !batch->colors.empty();
00119 WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get());
00120 }
00121 for (size_t i = 0; i < batch->points.size(); ++i) {
00122 WriteBinaryPcdPointCoordinate(batch->points[i].position,
00123 file_writer_.get());
00124 if (!batch->colors.empty()) {
00125 WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]),
00126 file_writer_.get());
00127 }
00128 ++num_points_;
00129 }
00130 next_->Process(std::move(batch));
00131 }
00132
00133 }
00134 }