cartographer源码分析(37)-io-pcd_writing_points_processor.h

最新推荐文章于 2021-09-15 07:00:00 发布

slamcode

最新推荐文章于 2021-09-15 07:00:00 发布

阅读量874

点赞数

CC 4.0 BY-SA版权

分类专栏： cartographer源码分析

本文链接：https://blog.youkuaiyun.com/learnmoreonce/article/details/76358959

cartographer源码分析专栏收录该内容

56 篇文章

订阅专栏

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


文件:pcd_writing_points_processor.h


#include <fstream>

#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_processor.h"
/*
点云是目标表面特性的海量点集合。
根据激光测量原理得到的点云，包括三维坐标（XYZ）和激光反射强度（Intensity）。
根据摄影测量原理得到的点云，包括三维坐标（XYZ）和颜色信息（RGB）。
结合激光测量和摄影测量原理得到点云，包括三维坐标（XYZ）、激光反射强度（Intensity）和颜色信息（RGB）。
在获取物体表面每个采样点的空间坐标后，得到的是一个点的集合，称之为“点云”(Point Cloud)。

pcd文件存储了每个点的x,y,z坐标以及r,g,b,a颜色信息

*/
namespace cartographer {
namespace io {

/*
PcdWritingPointsProcessor是PointsProcessor的第九个子类(9)
.
PcdWritingPointsProcessor类将点云按照pcd格式存储在pcd文件中.
1,构造函数初始化一个文件类名,
2,成员函数FromDictionary 从配置文件读取 文件名
3,Process()和Flush()负责写入文件
4,数据成员next_指向 下一阶段的PointsProcessor点云处理类
*/
// Streams a PCD file to disk. The header is written in 'Flush'.
class PcdWritingPointsProcessor : public PointsProcessor {
 public:
  constexpr static const char* kConfigurationFileActionName = "write_pcd";
  PcdWritingPointsProcessor(std::unique_ptr<FileWriter> file_writer,
                            PointsProcessor* next);

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

  ~PcdWritingPointsProcessor() override {}

  PcdWritingPointsProcessor(const PcdWritingPointsProcessor&) = delete;
  PcdWritingPointsProcessor& operator=(const PcdWritingPointsProcessor&) =
      delete;

  void Process(std::unique_ptr<PointsBatch> batch) override;
  FlushResult Flush() override;

 private:
  PointsProcessor* const next_;

  int64 num_points_;
  bool has_colors_;
  std::unique_ptr<FileWriter> file_writer_;
};

}  // namespace io
}  // namespace cartographer

pcd_writing_points_processor.cc


#include "cartographer/io/pcd_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 {

/*
PCD文件由文件头和数据域组成，文件头存储了点云数据的字段格式信息，如： 
======================================== 
# .PCD v0.7 - Point Cloud Data file format 
VERSION 0.7 
FIELDS x y z rgba 
SIZE 4 4 4 4 
TYPE F F F U 
COUNT 1 1 1 1 
WIDTH 640 
HEIGHT 480 
VIEWPOINT 0 0 0 0 1 0 0 
POINTS 307200 
DATA binary 
//下面是点云数据段 
123.1 21.1 64.0
...
======================================== 

*/
// Writes the PCD header claiming 'num_points' will follow it into
// 'output_file'.
void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
                          FileWriter* const file_writer) {
  string color_header_field = !has_color ? "" : " rgb";
  string color_header_type = !has_color ? "" : " U";
  string color_header_size = !has_color ? "" : " 4";
  string color_header_count = !has_color ? "" : " 1";

  std::ostringstream stream;
  stream << "# generated by Cartographer\n"
         << "VERSION .7\n"
         << "FIELDS x y z" << color_header_field << "\n"
         << "SIZE 4 4 4" << color_header_size << "\n"
         << "TYPE F F F" << color_header_type << "\n"
         << "COUNT 1 1 1" << color_header_count << "\n"
         << "WIDTH " << std::setw(15) << std::setfill('0') << num_points << "\n"
         << "HEIGHT 1\n"
         << "VIEWPOINT 0 0 0 1 0 0 0\n"
         << "POINTS " << std::setw(15) << std::setfill('0') << num_points
         << "\n"
         << "DATA binary\n";
  const string out = stream.str();
  file_writer->WriteHeader(out.data(), out.size());
}

//{x,y,z}写入文件
void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
                                   FileWriter* const file_writer) {
  char buffer[12];
  memcpy(buffer, &point[0], sizeof(float));
  memcpy(buffer + 4, &point[1], sizeof(float)); //64机,float:4位
  memcpy(buffer + 8, &point[2], sizeof(float));
  CHECK(file_writer->Write(buffer, 12));
}

void WriteBinaryPcdPointColor(const Color& color,
                              FileWriter* const file_writer) {
  char buffer[4];
  buffer[0] = color[2];
  buffer[1] = color[1];
  buffer[2] = color[0];
  buffer[3] = 0;
  CHECK(file_writer->Write(buffer, 4));
}

}  // namespace

std::unique_ptr<PcdWritingPointsProcessor>
PcdWritingPointsProcessor::FromDictionary(
    FileWriterFactory file_writer_factory,
    common::LuaParameterDictionary* const dictionary,
    PointsProcessor* const next) 

{
  return common::make_unique<PcdWritingPointsProcessor>(
      file_writer_factory(dictionary->GetString("filename")), next);
}

PcdWritingPointsProcessor::PcdWritingPointsProcessor(
    std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
    : next_(next),
      num_points_(0),
      has_colors_(false),
      file_writer_(std::move(file_writer)) {}

PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush()

 {
  WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get());
  //调用Flush()才写入head信息:此时才知道num_points_的值
  CHECK(file_writer_->Close());

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

    case FlushResult::kRestartStream://不能多次传递同一数据.
      LOG(FATAL) << "PCD generation must be configured to occur after any "
                    "stages that require multiple passes.";
  }
  LOG(FATAL);
}

/*
核心函数,按照{x,y,z}写入文件
*/
void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) 

{
  if (batch->points.empty()) {
    next_->Process(std::move(batch));
    return;
  }

  if (num_points_ == 0) {
    has_colors_ = !batch->colors.empty();
    WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get());
  }
  for (size_t i = 0; i < batch->points.size(); ++i) {
    WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get());
    if (!batch->colors.empty()) {
      WriteBinaryPcdPointColor(batch->colors[i], file_writer_.get());
    }
    ++num_points_;//统计处理了多少points
  }
  next_->Process(std::move(batch));
}

}  // namespace io
}  // namespace cartographer