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/ply_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 WriteBinaryPlyHeader(const bool has_color, const bool has_intensities,
00036 const std::vector<std::string>& comments,
00037 const int64 num_points,
00038 FileWriter* const file_writer) {
00039 const std::string color_header = !has_color ? ""
00040 : "property uchar red\n"
00041 "property uchar green\n"
00042 "property uchar blue\n";
00043 const std::string intensity_header =
00044 !has_intensities ? "" : "property float intensity\n";
00045 std::ostringstream stream;
00046 stream << "ply\n"
00047 << "format binary_little_endian 1.0\n"
00048 << "comment generated by Cartographer\n";
00049 for (const std::string& comment : comments) {
00050 stream << "comment " << comment << "\n";
00051 }
00052 stream << "element vertex " << std::setw(15) << std::setfill('0')
00053 << num_points << "\n"
00054 << "property float x\n"
00055 << "property float y\n"
00056 << "property float z\n"
00057 << color_header << intensity_header << "end_header\n";
00058 const std::string out = stream.str();
00059 CHECK(file_writer->WriteHeader(out.data(), out.size()));
00060 }
00061
00062 void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
00063 FileWriter* const file_writer) {
00064
00065 char buffer[12];
00066 memcpy(buffer, &point[0], sizeof(float));
00067 memcpy(buffer + 4, &point[1], sizeof(float));
00068 memcpy(buffer + 8, &point[2], sizeof(float));
00069 CHECK(file_writer->Write(buffer, 12));
00070 }
00071
00072 void WriteBinaryIntensity(const float intensity,
00073 FileWriter* const file_writer) {
00074
00075 CHECK(file_writer->Write(reinterpret_cast<const char*>(&intensity),
00076 sizeof(float)));
00077 }
00078
00079 void WriteBinaryPlyPointColor(const Uint8Color& color,
00080 FileWriter* const file_writer) {
00081 CHECK(file_writer->Write(reinterpret_cast<const char*>(color.data()),
00082 color.size()));
00083 }
00084
00085 }
00086
00087 std::unique_ptr<PlyWritingPointsProcessor>
00088 PlyWritingPointsProcessor::FromDictionary(
00089 const FileWriterFactory& file_writer_factory,
00090 common::LuaParameterDictionary* const dictionary,
00091 PointsProcessor* const next) {
00092 return absl::make_unique<PlyWritingPointsProcessor>(
00093 file_writer_factory(dictionary->GetString("filename")),
00094 std::vector<std::string>(), next);
00095 }
00096
00097 PlyWritingPointsProcessor::PlyWritingPointsProcessor(
00098 std::unique_ptr<FileWriter> file_writer,
00099 const std::vector<std::string>& comments, PointsProcessor* const next)
00100 : next_(next),
00101 comments_(comments),
00102 num_points_(0),
00103 has_colors_(false),
00104 file_(std::move(file_writer)) {}
00105
00106 PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() {
00107 WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, num_points_,
00108 file_.get());
00109 CHECK(file_->Close()) << "Closing PLY file_writer failed.";
00110
00111 switch (next_->Flush()) {
00112 case FlushResult::kFinished:
00113 return FlushResult::kFinished;
00114
00115 case FlushResult::kRestartStream:
00116 LOG(FATAL) << "PLY generation must be configured to occur after any "
00117 "stages that require multiple passes.";
00118 }
00119 LOG(FATAL);
00120 }
00121
00122 void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
00123 if (batch->points.empty()) {
00124 next_->Process(std::move(batch));
00125 return;
00126 }
00127
00128 if (num_points_ == 0) {
00129 has_colors_ = !batch->colors.empty();
00130 has_intensities_ = !batch->intensities.empty();
00131 WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, 0,
00132 file_.get());
00133 }
00134 if (has_colors_) {
00135 CHECK_EQ(batch->points.size(), batch->colors.size())
00136 << "First PointsBatch had colors, but encountered one without. "
00137 "frame_id: "
00138 << batch->frame_id;
00139 }
00140 if (has_intensities_) {
00141 CHECK_EQ(batch->points.size(), batch->intensities.size())
00142 << "First PointsBatch had intensities, but encountered one without. "
00143 "frame_id: "
00144 << batch->frame_id;
00145 }
00146
00147 for (size_t i = 0; i < batch->points.size(); ++i) {
00148 WriteBinaryPlyPointCoordinate(batch->points[i].position, file_.get());
00149 if (has_colors_) {
00150 WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get());
00151 }
00152 if (has_intensities_) {
00153 WriteBinaryIntensity(batch->intensities[i], file_.get());
00154 }
00155 ++num_points_;
00156 }
00157 next_->Process(std::move(batch));
00158 }
00159
00160 }
00161 }