Please select Into the mobile phone version | Continue to access the computer ver.
Products + Follow Have 7 Audience + New Thread

HAP文件lvx2格式C++解析参考

[Copy Link]
Author: smallErLang | Time: 2023-2-9 16:43:01 | HAP|
0 1862

1

Threads

2

Posts

33

Credits

Kindergarten

Rank: 1

Credits
33
Posted on 2023-2-9 16:43:01| All floors |Read mode
Last edited by smallErLang In 2023-2-9 16:42 Editor

#include <string>
#include <iostream>
#include <sstream>
#include <cstdint>
#include <vector>
#include <fstream>

#define FRM_POINTS_COUNT 45216

#pragma pack(push, 1)
class PubHeader
{
public:
    PubHeader()
    {

    }
    ~PubHeader()
    {

    }

    char singnate[16];
    char ver[4];
    std::uint32_t magic_doce;
};

class PriHeader
{
public:
    PriHeader()
    {

    }
    ~PriHeader()
    {

    }

    std::uint32_t frame_duration;
    std::uint8_t device_count;
};

class DeviceInfo
{
public:
    DeviceInfo()
    {

    }
    ~DeviceInfo()
    {

    }

    std::uint8_t lidar_broadcast_code[16];
    std::uint8_t hub_brocast_code[16];
    std::uint32_t lidar_id;
    std::uint8_t lidar_type;
    std::uint8_t device_type;
    std::uint8_t extrinsic_enable;
    float roll;
    float pitch;
    float yaw;
    float x;
    float y;
    float z;
};

class FrameHeader
{
public:
    FrameHeader()
    {

    }
    ~FrameHeader()
    {

    }

    std::uint64_t curr_offset;
    std::uint64_t next_offset;
    std::uint64_t frame_idx;
};

class BasePackHeader
{
public:
    BasePackHeader()
    {

    }
    ~BasePackHeader()
    {

    }

    std::uint8_t version;
    std::uint32_t lidar_id;
    std::uint8_t lidar_type;
    std::uint8_t timestamp_type;
    std::uint8_t timestamp[8];
    std::uint16_t udp_counter;
    std::uint8_t data_type;
    std::uint32_t length;
    std::uint8_t frame_counter;
    std::uint8_t reserve[4];
};

class ExtendRowPoint
{
public:
    ExtendRowPoint()
    {

    }
    ~ExtendRowPoint()
    {

    }

    std::int32_t x;
    std::int32_t y;
    std::int32_t z;
    std::uint8_t reflectivity;
    std::uint8_t tag;
};

class ExtendHalfRowPoint
{
public:
    ExtendHalfRowPoint()
    {

    }
    ~ExtendHalfRowPoint()
    {

    }

    std::int16_t x;
    std::int16_t y;
    std::int16_t z;
    std::uint8_t reflectivity;
    std::uint8_t tag;
};

class BasePackDetail
{
public:
    BasePackDetail()
    {

    }
    ~BasePackDetail()
    {

    }

    BasePackHeader header;
    std::vector<ExtendRowPoint> raw_point;
};

class BaseHalfPackDetail
{
public:
    BaseHalfPackDetail()
    {

    }
    ~BaseHalfPackDetail()
    {

    }

    BasePackHeader header;
    std::vector<ExtendHalfRowPoint> raw_point;
};
#pragma pack(pop)

int main(int argc,char* argv[])
{
    if (argc < 2)
    {
        std::cout << "Usage:exe [lvx2 file]" << std::endl;

        return 0;
    }

    std::string lvx2file = argv[1];

    std::ifstream inf_lvx2(lvx2file, std::ios_base::binary);
    if (!inf_lvx2)
    {
        std::cout << "Error:Can not open file---" << lvx2file << std::endl;

        return 0;
    }

    PubHeader pubheader;
    inf_lvx2.read((char*)(&pubheader), sizeof(pubheader));

    std::cout << "File singnate:" << pubheader.singnate << std::endl;
    std::cout << "Version:" << pubheader.ver[0] << "." << pubheader.ver[1] << "." << pubheader.ver[2] << "." << pubheader.ver[3] << std::endl;
    std::cout << "Magic code:" << std::hex << pubheader.magic_doce << std::dec << std::endl;

    PriHeader priheader;
    inf_lvx2.read((char*)(&priheader), sizeof(priheader));
   
    std::cout << "Frame duration:" << priheader.frame_duration << std::endl;
    std::cout << "Device count:" << (int)priheader.device_count << std::endl;

    for (int i = 0; i < priheader.device_count; i++)
    {
        DeviceInfo devinfo;
        inf_lvx2.read((char*)(&devinfo), sizeof(devinfo));

        std::cout << "Lidar type:" << (int)devinfo.lidar_type << std::endl;
        std::cout << "Device type:" << (int)devinfo.device_type << std::endl;
        std::cout << "Broadcast code:" << devinfo.lidar_broadcast_code << std::endl;
    }


    std::vector<float> fdata;
    fdata.reserve(FRM_POINTS_COUNT * 1.5 * 5);

    int timems = 0;
   
    while (!inf_lvx2.eof())
    {
        FrameHeader fraheader;
        inf_lvx2.read((char*)(&fraheader), sizeof(fraheader));

//         std::cout << "Frm curr offset:" << fraheader.curr_offset << std::endl;
//         std::cout << "Next offset:" << fraheader.next_offset << std::endl;
//         std::cout << "Frm idx:" << fraheader.frame_idx << std::endl;

        int bindx = 0;

        while (bindx < fraheader.next_offset - fraheader.curr_offset - sizeof(fraheader))
        {
            BasePackHeader pheader;
            inf_lvx2.read((char*)(&pheader), sizeof(pheader));

            bindx += sizeof(pheader);

//             std::cout << "Data type:" << (int)pheader.data_type << std::endl;
//             std::cout << "Length:" << pheader.length << std::endl;
//             std::cout << "Frm counter:" << (int)pheader.frame_counter << std::endl;

            if (pheader.data_type == 1)
            {
                int pcount = pheader.length / 14;
                for (int i = 0; i < pcount; i++)
                {
                    ExtendRowPoint pdetail;
                    inf_lvx2.read((char*)(&pdetail), sizeof(pdetail));

                    fdata.push_back(pdetail.x * 0.001f);
                    fdata.push_back(pdetail.y * 0.001f);
                    fdata.push_back(pdetail.z * 0.001f);
                    fdata.push_back(pdetail.reflectivity);
                    fdata.push_back(pdetail.tag);
                }
            }
            else if (pheader.data_type == 2)
            {
                int pcount = pheader.length / 8;
                for (int i = 0; i < pcount; i++)
                {
                    ExtendHalfRowPoint pdetail;
                    inf_lvx2.read((char*)(&pdetail), sizeof(pdetail));

                    fdata.push_back(pdetail.x * 0.01f);
                    fdata.push_back(pdetail.y * 0.01f);
                    fdata.push_back(pdetail.z * 0.01f);
                    fdata.push_back(pdetail.reflectivity);
                    fdata.push_back(pdetail.tag);
                }
            }
            else
            {
                std::cout << "Error:Can not surport this data type---" << (int)pheader.data_type << std::endl;
                break;
            }

            if (fdata.size() >= FRM_POINTS_COUNT * 5)
            {

                std::cout << "Frm " << timems / 100 << std::endl;

                std::vector<float>().swap(fdata);
                fdata.reserve(FRM_POINTS_COUNT * 1.5 * 5);
            }

            bindx += pheader.length;
        }
    }

    inf_lvx2.close();

    return 0;
}
Reply

Use props Report

You need to log in before you can reply Login | Register

Credit Rules

Quick Reply Back to top