当前位置：首页 > article >正文

C++手写PCD文件

article 2024/11/24 9:32:22

前言

一般pcd读写只需要调pcl库接口，直接用pcl的结构写就好了

这里是不依赖pcl库的写入方法
主要是开头写一个header
注意字段大小，类型不要写错

结构定义

写入点需要与header中定义一致
这里用的RoboSense的结构写demo
加了个1字节对齐

struct PointXYZIT {
  float x;
  float y;
  float z;
  uint16_t intensity;
  double timestamp;
} __attribute__((packed));

template <typename PointType>
class PointCloudT {
 public:
  typedef PointType PointT;
  typedef std::vector<PointT> VectorT;

  uint32_t height = 0;
  uint32_t width = 0;
  double timestamp = 0.0;
  uint32_t seq = 0;

  VectorT points;
};

代码实现

int record_pcd(PointCloudT<PointXYZIT> &point_cloud) {
  static int frame_id = 0;
  frame_id++;
  std::string pcd_file = "frame_" + std::to_string(frame_id) + ".pcd";
  int fd = open(pcd_file.c_str(), O_WRONLY | O_CREAT | O_TRUNC, 0644);
  const char *pcd_header =
        "# .PCD v.7 - Point Cloud Data file format\n"
        "FIELDS "
        "x y z intensity timestamp\n"
        "SIZE 4 4 4 2 8\n"
        "TYPE F F F U F\n"
        "COUNT 1 1 1 1 1\n"
        "WIDTH %zu\n"
        "HEIGHT 1\n"
        "VIEWPOINT 0 0 0 1 0 0 0\n"
        "POINTS %zu\n"
        "DATA binary\n";
  char header_buffer_[400];
  memset(header_buffer_, 0, sizeof(header_buffer_));
  uint64_t write_size =
      snprintf(header_buffer_, sizeof(header_buffer_), pcd_header, point_cloud.width, point_cloud.width);
  assert(write_size > 0 && write_size < sizeof(header_buffer_));
  int r = write(fd, header_buffer_, write_size);
  assert(r >= 0);
  for (int i = 0; i < point_cloud.width; i++) {
    r = write(fd, &point_cloud.points[i], sizeof(PointXYZIT));
    assert(r >= 0);
  }
  close(fd);
  fd = -1;
  return 0;
}