livox-SDK lvx格式转pcd、las、txt

#include "LvxReader.h" #include <memory> #include <tuple> #include <iostream> #include <time.h> #include <assert.h> #pragma region Global Container map<DeviceType, string> DeviceTypeNameMap { { kDeviceTypeHub,"LiDAR Hub"}, { kDeviceTypeLidarMid40,"Mid-40/Mid-100"}, { kDeviceTypeLidarTele,"Tele-15"}, { kDeviceTypeLidarHorizon,"Horizon"}, { kDeviceTypeLidarMid70,"Mid-70"}, { kDeviceTypeLidarAvia,"Avia"} }; map<TimestampType, string> TimestampTypeNameMap { // ref: https://github.com/Livox-SDK/Livox-SDK/wiki/Livox-SDK-Communication-Protocol //3.2 Time Stamp {#timestamp} { kTimestampTypeNoSync, "No sync signal mode" }, { kTimestampTypePtp, "1588v2.0 PTP sync mode" }, { kTimestampTypeRsvd, "Reserved use" }, { kTimestampTypePpsGps, "pps+gps sync mode" }, { kTimestampTypePps, "pps only sync mode" }, { kTimestampTypeUnknown,"Unknown mode" } }; map< PointDataType, pair<uint16_t, uint32_t> > PointBytesInfoMap { { kCartesian,{ RAW_POINT_NUM, sizeof(LivoxRawPoint) } }, { kSpherical,{ RAW_POINT_NUM, sizeof(LivoxSpherPoint) } }, { kExtendCartesian,{ SINGLE_POINT_NUM, sizeof(LivoxExtendRawPoint) } }, { kExtendSpherical,{ SINGLE_POINT_NUM, sizeof(LivoxExtendSpherPoint) } }, { kDualExtendCartesian,{ DUAL_POINT_NUM, sizeof(LivoxDualExtendRawPoint) } }, { kDualExtendSpherical,{ DUAL_POINT_NUM, sizeof(LivoxDualExtendSpherPoint) } }, { kTripleExtendCartesian,{ TRIPLE_POINT_NUM, sizeof(LivoxTripleExtendRawPoint) } }, { kTripleExtendSpherical,{ TRIPLE_POINT_NUM, sizeof(LivoxTripleExtendSpherPoint) } }, { kImu,{ IMU_POINT_NUM, sizeof(LivoxImuPoint) } } // LivoxPoint, Standard point cloud format }; map< PointDataType, string > PointTypeNameMap { { kCartesian, "Cartesian coordinate point cloud" }, { kSpherical, "Spherical coordinate point cloud" }, { kExtendCartesian, "Extend cartesian coordinate point cloud" }, { kExtendSpherical, "Extend spherical coordinate point cloud" }, { kDualExtendCartesian, "Dual extend cartesian coordinate point cloud" }, { kDualExtendSpherical, "Dual extend spherical coordinate point cloud" }, { kTripleExtendCartesian, "Triple extend cartesian coordinate point cloud" }, { kTripleExtendSpherical, "Triple extend spherical coordinate point cloud" }, { kImu, "IMU data" }, { kMaxPointDataType, "Max Point Data Type" } }; #pragma endregion LvxReader::LvxReader() : cur_frame_index_(0), cur_offset_(0), total_points_num_(0), frame_duration_(kDefaultFrameDurationTime) { } LvxReader::LvxReader(string filename, LogMode mode) : cur_frame_index_(0), cur_offset_(0), total_points_num_(0), frame_duration_(kDefaultFrameDurationTime), file_name_(filename), logmode_(mode) { } LvxReader::~LvxReader() { } size_t LvxReader::getBinFileSize(string filepath) { FILE *fp=fopen(filepath.c_str(),"r"); //printf("getBinFileSize : %s\n", filepath.c_str()); if(!fp) return -1; fseek(fp,0L,SEEK_END); int size=ftell(fp); fclose(fp); //printf("getBinFileSize : %d\n", size); return size; } bool LvxReader::InitLvxFileReader() { if (logmode_ > 0) { printf("Initial Lvx File Reader from %s ...\n", file_name_.c_str()); } file_size_ = getBinFileSize(file_name_); lvx_file_.open(file_name_, std::ios::in | std::ios::binary); if (!lvx_file_.is_open()) { return false; } if (logmode_ > 1) { printf(" file size: %.3f KB, %.3f MB, %.3f GB.\n", file_size_ / 1024, file_size_ / (1024*1024), file_size_ / (powl(2, 30))); } if (logmode_ > 0) { printf("Initial Lvx File Reader from %s Done!\n", file_name_.c_str()); cout << endl; } return true; } void LvxReader::ReadLvxFileHeader() { if (logmode_ > 0) cout << "Read Lvx File Header..." << endl; int header_size = sizeof(LvxFilePublicHeader) + sizeof(LvxFilePrivateHeader); std::unique_ptr<char[]> read_buffer(new char[READ_BUFFER_LEN]); lvx_file_.read((char*)read_buffer.get(), header_size); memcpy(&lvx_file_public_header_, read_buffer.get() + cur_offset_, sizeof(LvxFilePublicHeader)); cur_offset_ += sizeof(LvxFilePublicHeader); if (logmode_ > 2) { printf(" File Signature: %s\n", lvx_file_public_header_.signature); printf(" Version-A: %d\n", lvx_file_public_header_.version[0]); printf(" Version-B: %d\n", lvx_file_public_header_.version[1]); printf(" Version-C: %d\n", lvx_file_public_header_.version[2]); printf(" Version-D: %d\n", lvx_file_public_header_.version[3]); printf(" Magic Code: %x\n", lvx_file_public_header_.magic_code); } //assert(MAGIC_CODE == lvx_file_public_header_.magic_code); memcpy(&lvx_file_private_header_, read_buffer.get() + cur_offset_, sizeof(LvxFilePrivateHeader)); cur_offset_ += sizeof(LvxFilePrivateHeader); if (logmode_ > 1) { printf(" Per Frame Duration: %dms\n", lvx_file_private_header_.frame_duration); printf(" Device Count: %d\n", lvx_file_private_header_.device_count); } int device_num = lvx_file_private_header_.device_count; device_info_vec_.resize(device_num); int device_info_size = device_num * sizeof(LvxDeviceInfo); lvx_file_.read((char*)read_buffer.get() + cur_offset_, device_info_size); if (logmode_ > 1) { printf(" LiDAR SN Code, Hub SN Code, Device Index, Device Type, Extrinsic Enable, Roll, Pitch, Yaw, X, Y, Z \n"); } for (int i = 0; i < device_num; ++i) { memcpy(&device_info_vec_[i], read_buffer.get() + cur_offset_, sizeof(LvxDeviceInfo)); cur_offset_ += sizeof(LvxDeviceInfo); if (logmode_ > 1) { // extrinsic is read from the lvx file printf(" %s, %s, %d, %s, %s, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f \n", device_info_vec_[i].lidar_broadcast_code, device_info_vec_[i].hub_broadcast_code, device_info_vec_[i].device_index, DeviceTypeNameMap[DeviceType(device_info_vec_[i].device_type)].c_str(), device_info_vec_[i].extrinsic_enable ? "Yes" : "No", device_info_vec_[i].roll, device_info_vec_[i].pitch, device_info_vec_[i].yaw, device_info_vec_[i].x, device_info_vec_[i].y, device_info_vec_[i].z ); } } if (logmode_ > 0) { cout << "Read Lvx File Header Done!" << endl; cout << endl; } } void LvxReader::ReadFramesFromLvxFile() { if (logmode_ > 0) printf("Read Frames From Lvx File %s...\n", file_name_.c_str()); while (cur_offset_ < file_size_) { // read �� memcpy ͬ�����֣���������� uint64_t cur_frame_offset = 0; Frame cur_frame = { 0 }; std::unique_ptr<char[]> read_buffer(new char[READ_BUFFER_LEN]); lvx_file_.read((char*)read_buffer.get(), sizeof(FrameHeader)); memcpy(&cur_frame.header, read_buffer.get(), sizeof(FrameHeader)); cur_frame_offset += sizeof(FrameHeader); assert(cur_offset_ == cur_frame.current_offset); // read each package from package 0 - package N while (cur_offset_ + cur_frame_offset < cur_frame.next_offset) { LvxBasePackDetail_parse cur_pack; lvx_file_.read((char*)read_buffer.get() + cur_frame_offset, sizeof(LvxBasePackHeader)); memcpy(&cur_pack.header, read_buffer.get() + cur_frame_offset, sizeof(LvxBasePackHeader)); cur_frame_offset += sizeof(LvxBasePackHeader); int data_type = cur_pack.data_type; uint32_t points_byte_size = 0; /*switch (data_type) { case PointDataType::kCartesian: points_byte_size = RAW_POINT_NUM * sizeof(LivoxRawPoint); total_points_num_ += RAW_POINT_NUM; break; case PointDataType::kSpherical: points_byte_size = RAW_POINT_NUM * sizeof(LivoxSpherPoint); total_points_num_ += RAW_POINT_NUM; break; case PointDataType::kExtendCartesian: points_byte_size = SINGLE_POINT_NUM * sizeof(LivoxExtendRawPoint); total_points_num_ += SINGLE_POINT_NUM; break; case PointDataType::kExtendSphe