From 111d7bb5aac7c8ba6d5d3125de2d2fe4ddcec2e4 Mon Sep 17 00:00:00 2001
From: Marco Feuerstein <mfeuerstein@lyft.com>
Date: Tue, 26 Mar 2019 11:33:03 +0100
Subject: [PATCH] Ability to pass additional comments to ply header. (#1549)

---
 .../io/ply_writing_points_processor.cc        | 21 +++++++++++++------
 .../io/ply_writing_points_processor.h         |  2 ++
 2 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc
index 6c13d23..8b0ecdb 100644
--- a/cartographer/io/ply_writing_points_processor.cc
+++ b/cartographer/io/ply_writing_points_processor.cc
@@ -33,6 +33,7 @@ namespace {
 // Writes the PLY header claiming 'num_points' will follow it into
 // 'output_file'.
 void WriteBinaryPlyHeader(const bool has_color, const bool has_intensities,
+                          const std::vector<std::string>& comments,
                           const int64 num_points,
                           FileWriter* const file_writer) {
   const std::string color_header = !has_color ? ""
@@ -44,8 +45,11 @@ void WriteBinaryPlyHeader(const bool has_color, const bool has_intensities,
   std::ostringstream stream;
   stream << "ply\n"
          << "format binary_little_endian 1.0\n"
-         << "comment generated by Cartographer\n"
-         << "element vertex " << std::setw(15) << std::setfill('0')
+         << "comment generated by Cartographer\n";
+  for (const std::string& comment : comments) {
+    stream << "comment " << comment << "\n";
+  }
+  stream << "element vertex " << std::setw(15) << std::setfill('0')
          << num_points << "\n"
          << "property float x\n"
          << "property float y\n"
@@ -86,18 +90,22 @@ PlyWritingPointsProcessor::FromDictionary(
     common::LuaParameterDictionary* const dictionary,
     PointsProcessor* const next) {
   return absl::make_unique<PlyWritingPointsProcessor>(
-      file_writer_factory(dictionary->GetString("filename")), next);
+      file_writer_factory(dictionary->GetString("filename")),
+      std::vector<std::string>(), next);
 }
 
 PlyWritingPointsProcessor::PlyWritingPointsProcessor(
-    std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
+    std::unique_ptr<FileWriter> file_writer,
+    const std::vector<std::string>& comments, PointsProcessor* const next)
     : next_(next),
+      comments_(comments),
       num_points_(0),
       has_colors_(false),
       file_(std::move(file_writer)) {}
 
 PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() {
-  WriteBinaryPlyHeader(has_colors_, has_intensities_, num_points_, file_.get());
+  WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, num_points_,
+                       file_.get());
   CHECK(file_->Close()) << "Closing PLY file_writer failed.";
 
   switch (next_->Flush()) {
@@ -120,7 +128,8 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
   if (num_points_ == 0) {
     has_colors_ = !batch->colors.empty();
     has_intensities_ = !batch->intensities.empty();
-    WriteBinaryPlyHeader(has_colors_, has_intensities_, 0, file_.get());
+    WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, 0,
+                         file_.get());
   }
   if (has_colors_) {
     CHECK_EQ(batch->points.size(), batch->colors.size())
diff --git a/cartographer/io/ply_writing_points_processor.h b/cartographer/io/ply_writing_points_processor.h
index 8f07de9..7629587 100644
--- a/cartographer/io/ply_writing_points_processor.h
+++ b/cartographer/io/ply_writing_points_processor.h
@@ -26,6 +26,7 @@ class PlyWritingPointsProcessor : public PointsProcessor {
  public:
   constexpr static const char* kConfigurationFileActionName = "write_ply";
   PlyWritingPointsProcessor(std::unique_ptr<FileWriter> file_writer,
+                            const std::vector<std::string>& comments,
                             PointsProcessor* next);
 
   static std::unique_ptr<PlyWritingPointsProcessor> FromDictionary(
@@ -44,6 +45,7 @@ class PlyWritingPointsProcessor : public PointsProcessor {
  private:
   PointsProcessor* const next_;
 
+  std::vector<std::string> comments_;
   int64 num_points_;
   bool has_colors_;
   bool has_intensities_;