cartographer源码分析(33)-io-ply_writing_points_processor.h

源码可在https://github.com/learnmoreonce/SLAM 下载


文件:ply_writing_points_processor.h


#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_processor.h"

namespace cartographer {
namespace io {
/*
PlyWritingPointsProcessor是PointsProcessor的第五个子类(5).
PlyWritingPointsProcessor负责将点云point以PLY的格式写入磁盘.

*/
// Streams a PLY file to disk. The header is written in 'Flush'.
class PlyWritingPointsProcessor : public PointsProcessor {
 public:
  constexpr static const char* kConfigurationFileActionName = "write_ply";

  // 指定文件写入的工厂和流水线next指针。
  PlyWritingPointsProcessor(std::unique_ptr<FileWriter> file,
                            PointsProcessor* next);

  static std::unique_ptr<PlyWritingPointsProcessor> FromDictionary(
      FileWriterFactory file_writer_factory,
      common::LuaParameterDictionary* dictionary, PointsProcessor* next);

  ~PlyWritingPointsProcessor() override {}

  PlyWritingPointsProcessor(const PlyWritingPointsProcessor&) = delete;
  PlyWritingPointsProcessor& operator=(const PlyWritingPointsProcessor&) =
      delete;
//按照ply格式写点云信息 。然后直接流水线到下一PointsProcessor
  void Process(std::unique_ptr<PointsBatch> batch) override;
  FlushResult Flush() override;

 private:
  PointsProcessor* const next_;

  int64 num_points_;
  bool has_colors_;//是否是RGB
  std::unique_ptr<FileWriter> file_;
};

}  // namespace io
}  // namespace cartographer

ply_writing_points_processor.cc


#include "cartographer/io/ply_writing_points_processor.h"

#include <iomanip>
#include <sstream>
#include <string>

#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/io/points_batch.h"
#include "glog/logging.h"

namespace cartographer {
namespace io {

namespace {

/*
按照ply格式写入ply的head信息.此函数在最后写入.
why?num_points_只有最后才知道.之前尚未统计完成.

*/
// Writes the PLY header claiming 'num_points' will follow it into
// 'output_file'.
void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
                          FileWriter* const file_writer) {
  string color_header = !has_color ? ""
                                   : "property uchar red\n"
                                     "property uchar green\n"
                                     "property uchar blue\n";
  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')
         << num_points << "\n"
         << "property float x\n"
         << "property float y\n"
         << "property float z\n"
         << color_header << "end_header\n";
  const string out = stream.str();
  CHECK(file_writer->WriteHeader(out.data(), out.size()));
}

/*
往file_writer写入某一point对应的{x,y,z}
*/
void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
                                   FileWriter* const file_writer) {
  char buffer[12];
  memcpy(buffer, &point[0], sizeof(float));
  memcpy(buffer + 4, &point[1], sizeof(float));
  memcpy(buffer + 8, &point[2], sizeof(float));
  CHECK(file_writer->Write(buffer, 12));
}

/*
往file_writer写入rgb值
*/
void WriteBinaryPlyPointColor(const Color& color,
                              FileWriter* const file_writer) {
  CHECK(file_writer->Write(reinterpret_cast<const char*>(color.data()),
                           color.size()));
}

}  // namespace

/*
根据.lua文件的filename创建file_writer_factory
*/
std::unique_ptr<PlyWritingPointsProcessor>
PlyWritingPointsProcessor::FromDictionary(
    FileWriterFactory file_writer_factory,
    common::LuaParameterDictionary* const dictionary,
    PointsProcessor* const next) {
  return common::make_unique<PlyWritingPointsProcessor>(
      file_writer_factory(dictionary->GetString("filename")), next);
}

/*
构造函数,默认点云数是0,无rgb值
*/
PlyWritingPointsProcessor::PlyWritingPointsProcessor(
    std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
    : next_(next),
      num_points_(0),
      has_colors_(false),
      file_(std::move(file_writer)) {}

/*
在最后阶段才写入head,但不是在文件最后写入head,而是out_.seekp(0); 偏移到0处
*/
PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() {
  WriteBinaryPlyHeader(has_colors_, num_points_, file_.get());
  CHECK(file_->Close()) << "Closing PLY file_writer failed.";

  switch (next_->Flush()) {
    case FlushResult::kFinished:
      return FlushResult::kFinished;

    case FlushResult::kRestartStream:
      LOG(FATAL) << "PLY generation must be configured to occur after any "
                    "stages that require multiple passes.";
  }
  LOG(FATAL);
}

/*
真正的写入点云PointsBatch,整个类的核心在这个函数.
*/
void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
  if (batch->points.empty()) { //点云为空,则不写入磁盘,直接进行下一Process环节.
    next_->Process(std::move(batch));
    return;
  }

  if (num_points_ == 0) {//点云num为0则写入rgb,和head
    has_colors_ = !batch->colors.empty();
    WriteBinaryPlyHeader(has_colors_, 0, file_.get());
    //file_.get()获取指针指针对应的raw指针
  }
  if (has_colors_) { //rgb的vector长度和point的vecor长度必须一致.
    CHECK_EQ(batch->points.size(), batch->colors.size())
        << "First PointsBatch had colors, but encountered one without. "
           "frame_id: "
        << batch->frame_id;
  }

//函数核心,开始写入每一个point.
  for (size_t i = 0; i < batch->points.size(); ++i) {
    WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
    if (has_colors_) {
      WriteBinaryPlyPointColor(batch->colors[i], file_.get());
    }
    ++num_points_;
  }
  next_->Process(std::move(batch)); //继续流水线操作.
}

}  // namespace io
}  // namespace cartographer