|
|
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;
}
|
|
Posted on 2023-2-9 16:43:01