371 lines
14 KiB
C++
371 lines
14 KiB
C++
#include "LaserDataLoader.h"
|
|
#include <fstream>
|
|
#include <sstream>
|
|
#include <cstring>
|
|
#include <cstdlib>
|
|
#include "VrLog.h"
|
|
|
|
LaserDataLoader::LaserDataLoader()
|
|
{
|
|
m_lastError.clear();
|
|
}
|
|
|
|
LaserDataLoader::~LaserDataLoader()
|
|
{
|
|
}
|
|
|
|
int LaserDataLoader::LoadLaserScanData(const std::string& fileName,
|
|
std::vector<SVzNL3DLaserLine>& laserLines,
|
|
int& lineNum,
|
|
float& scanSpeed,
|
|
int& maxTimeStamp,
|
|
int& clockPerSecond)
|
|
{
|
|
LOG_INFO("Loading laser scan data from file: %s\n", fileName.c_str());
|
|
|
|
// 清空输出参数
|
|
laserLines.clear();
|
|
lineNum = 0;
|
|
scanSpeed = 0.0f;
|
|
maxTimeStamp = 0;
|
|
clockPerSecond = 0;
|
|
|
|
// 调用内部读取函数
|
|
int dataCalib = 0;
|
|
SVzNL3DLaserLine* rawData = ReadLaserScanPointFromFile_XYZ(fileName.c_str(),
|
|
&lineNum,
|
|
&scanSpeed,
|
|
&dataCalib,
|
|
&maxTimeStamp,
|
|
&clockPerSecond);
|
|
|
|
if (rawData == nullptr) {
|
|
m_lastError = "Failed to read laser scan data from file: " + fileName;
|
|
LOG_ERROR("Failed to load laser scan data from file: %s\n", fileName.c_str());
|
|
return ERR_CODE(FILE_ERR_NOEXIST);
|
|
}
|
|
|
|
// 将原始数据转换为对象向量格式
|
|
try {
|
|
laserLines.reserve(lineNum);
|
|
for (int i = 0; i < lineNum; i++) {
|
|
SVzNL3DLaserLine laserLine;
|
|
laserLine.nTimeStamp = rawData[i].nTimeStamp;
|
|
laserLine.nPositionCnt = rawData[i].nPositionCnt;
|
|
|
|
// 深拷贝p3DPosition数据
|
|
if (rawData[i].p3DPosition && rawData[i].nPositionCnt > 0) {
|
|
laserLine.p3DPosition = static_cast<SVzNL3DPosition*>(malloc(sizeof(SVzNL3DPosition) * rawData[i].nPositionCnt));
|
|
if (laserLine.p3DPosition) {
|
|
memcpy(laserLine.p3DPosition, rawData[i].p3DPosition, sizeof(SVzNL3DPosition) * rawData[i].nPositionCnt);
|
|
} else {
|
|
throw std::bad_alloc();
|
|
}
|
|
} else {
|
|
laserLine.p3DPosition = nullptr;
|
|
}
|
|
|
|
laserLines.push_back(laserLine);
|
|
}
|
|
|
|
// 释放原始数据内存
|
|
for (int i = 0; i < lineNum; i++) {
|
|
if (rawData[i].p3DPosition) {
|
|
free(rawData[i].p3DPosition);
|
|
}
|
|
}
|
|
free(rawData);
|
|
|
|
} catch (const std::exception& e) {
|
|
m_lastError = "Memory allocation error: " + std::string(e.what());
|
|
LOG_ERROR("Memory allocation error when loading laser data: %s\n", e.what());
|
|
|
|
// 释放已分配的内存
|
|
if (rawData) {
|
|
for (int i = 0; i < lineNum; i++) {
|
|
if (rawData[i].p3DPosition) {
|
|
free(rawData[i].p3DPosition);
|
|
}
|
|
}
|
|
free(rawData);
|
|
}
|
|
return ERR_CODE(DATA_ERR_INVALID);
|
|
}
|
|
|
|
LOG_INFO("Successfully loaded %d laser scan lines from file: %s\n", lineNum, fileName.c_str());
|
|
return SUCCESS;
|
|
}
|
|
|
|
int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
|
|
const std::vector<SVzNL3DLaserLine>& laserLines,
|
|
int lineNum,
|
|
float scanSpeed,
|
|
int maxTimeStamp,
|
|
int clockPerSecond)
|
|
{
|
|
LOG_INFO("Saving laser scan data to file: %s\n", fileName.c_str());
|
|
|
|
if (laserLines.empty() || lineNum <= 0) {
|
|
m_lastError = "Invalid input parameters for saving";
|
|
LOG_ERROR("Invalid parameters for saving laser data\n");
|
|
return ERR_CODE(DEV_ARG_INVAILD);
|
|
}
|
|
|
|
try {
|
|
std::ofstream sw(fileName);
|
|
if (!sw.is_open()) {
|
|
m_lastError = "Cannot open file for writing: " + fileName;
|
|
LOG_ERROR("Cannot open file for writing: %s\n", fileName.c_str());
|
|
return ERR_CODE(FILE_ERR_WRITE);
|
|
}
|
|
|
|
// 写入文件头
|
|
sw << "LineNum:" << lineNum << std::endl;
|
|
sw << "DataType: 0" << std::endl;
|
|
sw << "ScanSpeed:" << scanSpeed << std::endl;
|
|
sw << "PointAdjust: 1" << std::endl;
|
|
sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl;
|
|
|
|
// 写入每条扫描线数据
|
|
for (int line = 0; line < lineNum && line < static_cast<int>(laserLines.size()); line++) {
|
|
const SVzNL3DLaserLine& scanLine = laserLines[line]; // 使用对象引用而不是指针
|
|
|
|
sw << "Line_" << line << "_" << scanLine.nTimeStamp << "_" << scanLine.nPositionCnt << std::endl;
|
|
|
|
// 写入有效的3D点
|
|
for (int i = 0; i < scanLine.nPositionCnt; i++) {
|
|
const SVzNL3DPosition* pt3D = &scanLine.p3DPosition[i];
|
|
float x = static_cast<float>(pt3D->pt3D.x);
|
|
float y = static_cast<float>(pt3D->pt3D.y);
|
|
float z = static_cast<float>(pt3D->pt3D.z);
|
|
sw << "{ " << x << "," << y << "," << z << " }-";
|
|
sw << "{0,0}-{0,0}" << std::endl;
|
|
}
|
|
}
|
|
|
|
sw.close();
|
|
LOG_INFO("Successfully saved laser scan data to file: %s\n", fileName.c_str());
|
|
return SUCCESS;
|
|
|
|
} catch (const std::exception& e) {
|
|
m_lastError = "Error saving file: " + std::string(e.what());
|
|
LOG_ERROR("Error saving laser data to file: %s\n", e.what());
|
|
return ERR_CODE(FILE_ERR_WRITE);
|
|
}
|
|
}
|
|
|
|
void LaserDataLoader::FreeLaserScanData(std::vector<SVzNL3DLaserLine>& laserLines)
|
|
{
|
|
LOG_DEBUG("Freeing laser scan data, line count: %zu\n", laserLines.size());
|
|
|
|
if (!laserLines.empty()) {
|
|
// 释放每个对象中的p3DPosition内存
|
|
for (auto& scanLine : laserLines) {
|
|
if (scanLine.p3DPosition) {
|
|
free(scanLine.p3DPosition);
|
|
scanLine.p3DPosition = nullptr;
|
|
}
|
|
}
|
|
|
|
laserLines.clear();
|
|
}
|
|
|
|
LOG_DEBUG("Laser scan data freed successfully\n");
|
|
}
|
|
|
|
SVzNL3DLaserLine* LaserDataLoader::ReadLaserScanPointFromFile_XYZ(const char* fileName,
|
|
int* scanLineNum,
|
|
float* scanV,
|
|
int* dataCalib,
|
|
int* scanMaxStamp,
|
|
int* canClockUnit)
|
|
{
|
|
std::ifstream inputFile(fileName);
|
|
std::string linedata;
|
|
|
|
if (!inputFile.is_open()) {
|
|
m_lastError = "Cannot open file: " + std::string(fileName);
|
|
return nullptr;
|
|
}
|
|
|
|
SVzNL3DLaserLine* _scanLines = nullptr;
|
|
int lines = 0;
|
|
int dataElements = 4;
|
|
int firstIndex = -1;
|
|
|
|
// 数据文件版本检测
|
|
enum DataVersion { DATA_VER_OLD = 0, DATA_VER_NEW = 1 };
|
|
DataVersion dataFileVer = DATA_VER_OLD;
|
|
|
|
std::getline(inputFile, linedata); // 第一行
|
|
int lineNum = 0;
|
|
|
|
if (linedata.find("LineNum:") == 0) {
|
|
dataFileVer = DATA_VER_NEW;
|
|
sscanf(linedata.c_str(), "LineNum:%d", &lines);
|
|
if (lines == 0) {
|
|
m_lastError = "Invalid line number in file header";
|
|
return nullptr;
|
|
}
|
|
lineNum = lines;
|
|
_scanLines = static_cast<SVzNL3DLaserLine*>(malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)));
|
|
if (!_scanLines) {
|
|
m_lastError = "Memory allocation failed";
|
|
return nullptr;
|
|
}
|
|
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
|
if (scanLineNum) *scanLineNum = lines;
|
|
}
|
|
else if (linedata.find("LineNum_") == 0) {
|
|
dataFileVer = DATA_VER_OLD;
|
|
sscanf(linedata.c_str(), "LineNum_%d", &lines);
|
|
if (lines == 0) {
|
|
m_lastError = "Invalid line number in file header";
|
|
return nullptr;
|
|
}
|
|
lineNum = lines;
|
|
_scanLines = static_cast<SVzNL3DLaserLine*>(malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)));
|
|
if (!_scanLines) {
|
|
m_lastError = "Memory allocation failed";
|
|
return nullptr;
|
|
}
|
|
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
|
if (scanLineNum) *scanLineNum = lines;
|
|
}
|
|
|
|
if (_scanLines == nullptr) {
|
|
m_lastError = "Failed to allocate memory for scan lines";
|
|
return nullptr;
|
|
}
|
|
|
|
int ptNum = 0;
|
|
int lineIdx = -1;
|
|
SVzNL3DPosition* p3DPoint = nullptr;
|
|
|
|
if (dataFileVer == DATA_VER_NEW) {
|
|
while (getline(inputFile, linedata)) {
|
|
if (linedata.find("ScanSpeed:") == 0) {
|
|
double lineV = 0;
|
|
sscanf(linedata.c_str(), "ScanSpeed:%lf", &lineV);
|
|
if (scanV) *scanV = static_cast<float>(lineV);
|
|
}
|
|
else if (linedata.find("PointAdjust:") == 0) {
|
|
int ptAdjusted = 0;
|
|
sscanf(linedata.c_str(), "PointAdjust:%d", &ptAdjusted);
|
|
if (dataCalib) *dataCalib = ptAdjusted;
|
|
}
|
|
else if (linedata.find("MaxTimeStamp:") == 0) {
|
|
unsigned int maxTimeStamp = 0;
|
|
unsigned int timePerStamp = 0;
|
|
sscanf(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp);
|
|
if (scanMaxStamp) *scanMaxStamp = maxTimeStamp;
|
|
if (canClockUnit) *canClockUnit = timePerStamp;
|
|
}
|
|
else if (linedata.find("Line_") == 0) {
|
|
int lineIndex;
|
|
unsigned int timeStamp;
|
|
sscanf(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
|
|
if (firstIndex < 0) firstIndex = lineIndex;
|
|
|
|
lineIndex = lineIndex - firstIndex;
|
|
if ((lineIndex < 0) || (lineIndex >= lines)) break;
|
|
|
|
// 新扫描线
|
|
lineIdx++;
|
|
if (ptNum > 0) {
|
|
p3DPoint = static_cast<SVzNL3DPosition*>(malloc(sizeof(SVzNL3DPosition) * ptNum));
|
|
if (!p3DPoint) {
|
|
m_lastError = "Memory allocation failed for 3D points";
|
|
// 清理已分配的内存
|
|
for (int i = 0; i <= lineIdx; i++) {
|
|
if (_scanLines[i].p3DPosition) {
|
|
free(_scanLines[i].p3DPosition);
|
|
}
|
|
}
|
|
free(_scanLines);
|
|
return nullptr;
|
|
}
|
|
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
|
|
} else {
|
|
p3DPoint = nullptr;
|
|
}
|
|
_scanLines[lineIdx].nPositionCnt = 0;
|
|
_scanLines[lineIdx].nTimeStamp = timeStamp;
|
|
_scanLines[lineIdx].p3DPosition = p3DPoint;
|
|
}
|
|
else if (linedata.find("{") == 0) {
|
|
float X, Y, Z;
|
|
float leftX, leftY;
|
|
float rightX, rightY;
|
|
sscanf(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
|
int id = _scanLines[lineIdx].nPositionCnt;
|
|
if (id < ptNum && p3DPoint) {
|
|
p3DPoint[id].pt3D.x = X;
|
|
p3DPoint[id].pt3D.y = Y;
|
|
p3DPoint[id].pt3D.z = Z;
|
|
_scanLines[lineIdx].nPositionCnt = id + 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if (dataFileVer == DATA_VER_OLD) {
|
|
while (getline(inputFile, linedata)) {
|
|
if (linedata.find("DataElements_") == 0) {
|
|
sscanf(linedata.c_str(), "DataElements_%d", &dataElements);
|
|
if ((dataElements != 3) && (dataElements != 4)) break;
|
|
}
|
|
if (linedata.find("LineV_") == 0) {
|
|
double lineV = 0;
|
|
sscanf(linedata.c_str(), "LineV_%lf", &lineV);
|
|
if (scanV) *scanV = static_cast<float>(lineV);
|
|
}
|
|
else if (linedata.find("Line_") == 0) {
|
|
int lineIndex;
|
|
unsigned int timeStamp;
|
|
sscanf(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp);
|
|
|
|
if (firstIndex < 0) firstIndex = lineIndex;
|
|
lineIndex = lineIndex - firstIndex;
|
|
if ((lineIndex < 0) || (lineIndex >= lines)) break;
|
|
|
|
// 新扫描线
|
|
lineIdx++;
|
|
p3DPoint = static_cast<SVzNL3DPosition*>(malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM));
|
|
if (!p3DPoint) {
|
|
m_lastError = "Memory allocation failed for 3D points";
|
|
// 清理已分配的内存
|
|
for (int i = 0; i <= lineIdx; i++) {
|
|
if (_scanLines[i].p3DPosition) {
|
|
free(_scanLines[i].p3DPosition);
|
|
}
|
|
}
|
|
free(_scanLines);
|
|
return nullptr;
|
|
}
|
|
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
|
|
_scanLines[lineIdx].nPositionCnt = 0;
|
|
_scanLines[lineIdx].nTimeStamp = timeStamp;
|
|
_scanLines[lineIdx].p3DPosition = p3DPoint;
|
|
}
|
|
else if (linedata.find("(") == 0) {
|
|
float X, Y, Z;
|
|
int imageY = 0;
|
|
if (dataElements == 4) {
|
|
sscanf(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY);
|
|
} else {
|
|
sscanf(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z);
|
|
}
|
|
int id = _scanLines[lineIdx].nPositionCnt;
|
|
if (id < VZ_LASER_LINE_PT_MAX_NUM && p3DPoint) {
|
|
p3DPoint[id].pt3D.x = X;
|
|
p3DPoint[id].pt3D.y = Y;
|
|
p3DPoint[id].pt3D.z = Z;
|
|
_scanLines[lineIdx].nPositionCnt = id + 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
inputFile.close();
|
|
return _scanLines;
|
|
}
|