C++ 实现lvx格式的点云转换成pcd，不依赖sdk,根据协议解析lvx

以下是一个基于 Livox 官方协议的 C++ 实现，可以将 lvx 格式的点云转换成 PCD 格式。该实现不依赖于 Livox SDK，仅需要使用 C++ 的文件操作和数据结构处理等基本功能。

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

using namespace std;

struct LivoxLvxFileHeader {
    uint8_t magic_code[4];          // 文件头标识符，固定为“Livox”
    uint32_t file_version;          // 文件版本号，当前为 1
    uint32_t header_size;           // 文件头大小，固定为 256
    uint64_t data_offset;           // 数据体相对于文件头的偏移量，固定为 256
    uint64_t frame_duration;        // 数据帧时长，单位微秒
    uint64_t frame_duration_offset; // 数据帧时长偏移量，固定为 72
    uint64_t first_packet_offset;   // 第一个数据包偏移量，固定为 80
    uint64_t next_offset;           // 下一个 lvx 文件的偏移量，如果没有下一个文件，则为 0
    uint32_t file_number;           // 当前文件编号，固定为 1
    uint32_t total_file_number;     // 文件总数，固定为 1
    uint8_t device_sn[16];          // 设备序列号，16 个字节
    uint8_t device_type[8];         // 设备类型，8 个字节
    uint8_t reserved[128];          // 保留字段，128 个字节
};

struct LivoxLvxFilePacket {
    uint8_t signature[4];   // 包头签名，固定为“Livox”
    uint16_t version_major; // 包头版本号，当前为 1
    uint16_t version_minor;
    uint32_t packet_size;   // 包大小，包括包头和数据体
    uint64_t timestamp;     // 时间戳，单位微秒
    uint8_t data_type;      // 数据类型，0 表示点云数据
    uint8_t data_format;    // 数据格式，0 表示 XYZ 格式
    uint16_t lidar_id;      // Lidar ID，从 1 开始编号
    uint16_t reserved;      // 保留字段
    uint32_t data_size;     // 数据体大小，即点云数据大小
};

struct LivoxPointXYZ {
    float x;
    float y;
    float z;
};

bool read_lvx_header(const char* file_path, LivoxLvxFileHeader& header) {
    ifstream file(file_path, ios::binary);
    if (!file) {
        cerr << "Failed to open file: " << file_path << endl;
        return false;
    }

    file.read(reinterpret_cast<char*>(&header), sizeof(header));
    if (strncmp(reinterpret_cast<const char*>(header.magic_code), "Livox", 4) != 0) {
        cerr << "Invalid lvx file: " << file_path << endl;
        return false;
    }

    return true;
}

bool read_lvx_packet(ifstream& file, LivoxLvxFilePacket& packet) {
    file.read(reinterpret_cast<char*>(&packet), sizeof(packet));
    if (strncmp(reinterpret_cast<const char*>(packet.signature), "Livox", 4) != 0) {
        cerr << "Invalid lvx packet" << endl;
        return false;
    }
    if (packet.data_type != 0 || packet.data_format != 0) {
        cerr << "Unsupported lvx packet data type or format" << endl;
        return false;
    }

    return true;
}

bool read_lvx_point_cloud(const char* file_path, vector<LivoxPointXYZ>& point_cloud) {
    LivoxLvxFileHeader header;
    if (!read_lvx_header(file_path, header)) {
        return false;
    }

    ifstream file(file_path, ios::binary);
    if (!file) {
        cerr << "Failed to open file: " << file_path << endl;
        return false;
    }

    file.seekg(header.first_packet_offset, ios::beg);
    while (file.tellg() < header.data_offset + header.total_file_number * header.data_offset) {
        LivoxLvxFilePacket packet;
        if (!read_lvx_packet(file, packet)) {
            cerr << "Failed to read lvx packet" << endl;
            return false;
        }

        vector<LivoxPointXYZ> packet_point_cloud(packet.data_size / sizeof(LivoxPointXYZ));
        file.read(reinterpret_cast<char*>(packet_point_cloud.data()), packet.data_size);
        if (!file) {
            cerr << "Failed to read lvx packet point cloud" << endl;
            return false;
        }

        point_cloud.insert(point_cloud.end(), packet_point_cloud.begin(), packet_point_cloud.end());
    }

    return true;
}

bool write_pcd_file(const char* file_path, const vector<LivoxPointXYZ>& point_cloud) {
    ofstream file(file_path);
    if (!file) {
        cerr << "Failed to create file: " << file_path << endl;
        return false;
    }

    file << "# .PCD v0.7 - Point Cloud Data file format\n"
         << "VERSION 0.7\n"
         << "FIELDS x y z\n"
         << "SIZE 4 4 4\n"
         << "TYPE F F F\n"
         << "COUNT 1 1 1\n"
         << "WIDTH " << point_cloud.size() << "\n"
         << "HEIGHT 1\n"
         << "VIEWPOINT 0 0 0 1 0 0 0\n"
         << "POINTS " << point_cloud.size() << "\n"
         << "DATA ascii\n";

    for (const auto& point : point_cloud) {
        file << point.x << " " << point.y << " " << point.z << "\n";
    }

    return true;
}

int main(int argc, char** argv) {
    if (argc != 3) {
        cerr << "Usage: " << argv[0] << " lvx_file_path pcd_file_path" << endl;
        return 1;
    }

    vector<LivoxPointXYZ> point_cloud;
    if (!read_lvx_point_cloud(argv[1], point_cloud)) {
        return 1;
    }

    if (!write_pcd_file(argv[2], point_cloud)) {
        return 1;
    }

    return 0;
}

以上代码提供了一个完整的 lvx 格式点云转换成 PCD 格式的示例，可以通过传入命令行参数指定需要转换的 lvx 文件和输出的 PCD 文件。