C++ 实现lvx 格式点云转为pcd格式

下面是一个简单的 C++ 实现 lvx 格式点云转为 pcd 格式的示例代码：

#include <iostream>
#include <fstream>
#include <vector>
#include <cstring>

struct LivoxFileHeader {
    uint32_t magic_num;         // 魔数，固定值 0x5A5A5A5A
    uint32_t version_major;     // 版本号，主版本号
    uint32_t version_minor;     // 版本号，次版本号
    uint32_t frame_duration;    // 帧时长，单位微秒（us）
    uint64_t frame_index;       // 帧编号，从 0 开始
    uint32_t point_num;         // 点云数量
    uint32_t data_offset;       // 数据偏移量，从文件头开始
    uint32_t protocol_version;  // 协议版本
    uint32_t header_size;       // 文件头大小
};

struct LivoxPoint {
    float x;
    float y;
    float z;
    uint8_t r;
    uint8_t g;
    uint8_t b;
};

int main(int argc, char* argv[]) {
    if (argc < 3) {
        std::cout << "Usage: " << argv[0] << " input.lvx output.pcd" << std::endl;
        return -1;
    }

    // 打开输入文件
    std::ifstream input_file(argv[1], std::ios::binary);
    if (!input_file.is_open()) {
        std::cout << "Failed to open input file " << argv[1] << std::endl;
        return -1;
    }

    // 读取文件头部信息
    LivoxFileHeader header;
    input_file.read(reinterpret_cast<char*>(&header), sizeof(header));

    // 检查魔数和协议版本是否正确
    if (header.magic_num != 0x5A5A5A5A || header.protocol_version != 0x01010101) {
        std::cout << "Invalid lvx file" << std::endl;
        return -1;
    }

    // 计算每个点的偏移量
    std::streampos point_offset = header.data_offset + sizeof(LivoxPoint) * header.point_num;

    // 打开输出文件
    std::ofstream output_file(argv[2], std::ios::binary);
    if (!output_file.is_open()) {
        std::cout << "Failed to open output file " << argv[2] << std::endl;
        return -1;
    }

    // 写入 pcd 文件头
    output_file << "# .PCD v0.7 - Point Cloud Data file format\n";
    output_file << "VERSION 0.7\n";
    output_file << "FIELDS x y z rgb\n";
    output_file << "SIZE 4 4 4 4\n";
    output_file << "TYPE F F F U\n";
    output_file << "COUNT 1 1 1 1\n";
    output_file << "WIDTH " << header.point_num << "\n";
    output_file << "HEIGHT 1\n";
    output_file << "VIEWPOINT 0 0 0 1 0 0 0\n";
    output_file << "POINTS " << header.point_num << "\n";
    output_file << "DATA ascii\n";

    // 读取每个点的数据，并转换为 pcd 格式的数据结构
    input_file.seekg(point_offset);
    for (int i = 0; i < header.point_num; i++) {
        LivoxPoint point;
        input_file.read(reinterpret_cast<char*>(&point), sizeof(point));

        uint32_t rgb = (point.r << 16) | (point.g << 8) | point.b;
        float x = point.x / 1000.0f;
        float y = point.y / 1000.0f;
        float z = point.z / 1000.0f;

        output_file << x << " " << y << " " << z << " " << rgb << "\n";
    }

    std::cout << "Successfully converted " << header.point_num << " points from " << argv[1] << " to " << argv[2] << std::endl;

    return 0;
}

上面的代码使用 std::ifstream 和 std::ofstream 类来读写文件，使用了 C++11 中的强制类型转换 reinterpret_cast。需要注意的是，lvx 格式的数据结构可能会随着 Livox 3D 激光雷达的升级而发生变化，因此需要根据实际情况进行调整。