747 lines
29 KiB
C++
747 lines
29 KiB
C++
#include "LaserDataLoader.h"
|
||
#include <fstream>
|
||
#include <sstream>
|
||
#include <cstring>
|
||
#include <cstdlib>
|
||
#include <stdexcept>
|
||
#include <regex>
|
||
#include <cmath>
|
||
#include "VrLog.h"
|
||
#include <iomanip>
|
||
|
||
// 辅助函数：去除字符串末尾的 \r 字符（处理 Windows 格式的 CR LF 换行符）
|
||
static inline void TrimCarriageReturn(std::string& str)
|
||
{
|
||
if (!str.empty() && str.back() == '\r') {
|
||
str.pop_back();
|
||
}
|
||
}
|
||
|
||
LaserDataLoader::LaserDataLoader()
|
||
{
|
||
m_lastError.clear();
|
||
}
|
||
|
||
LaserDataLoader::~LaserDataLoader()
|
||
{
|
||
}
|
||
|
||
int LaserDataLoader::LoadLaserScanData(const std::string& fileName,
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& 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;
|
||
|
||
// 判断文件类型
|
||
std::ifstream inputFile(fileName);
|
||
if (!inputFile.is_open()) {
|
||
m_lastError = "Cannot open file: " + fileName;
|
||
LOG_ERROR("Cannot open file: %s\n", fileName.c_str());
|
||
return ERR_CODE(FILE_ERR_NOEXIST);
|
||
}
|
||
|
||
std::string line;
|
||
int result = SUCCESS;
|
||
|
||
// 包含DataType字段，检查数据类型
|
||
EVzResultDataType eDataType = keResultDataType_Invalid;
|
||
result = _GetLaserType(fileName, eDataType);
|
||
ERR_CODE_RETURN(result);
|
||
|
||
LOG_INFO("Laser data type: %d \n", eDataType);
|
||
|
||
SVzLaserLineData sLaserData;
|
||
memset(&sLaserData, 0, sizeof(SVzLaserLineData));
|
||
|
||
bool bFindLineNum = true;
|
||
int nLaserPointIdx = 0;
|
||
|
||
while (std::getline(inputFile, line)) {
|
||
// 去除行末的 \r 字符（处理 Windows 格式的 CR LF 换行符）
|
||
TrimCarriageReturn(line);
|
||
|
||
if (line.find("LineNum:") == 0) {
|
||
sscanf(line.c_str(), "LineNum:%d", &lineNum);
|
||
} else if (line.find("DataType:") == 0) {
|
||
|
||
} else if (line.find("Line_") == 0) {
|
||
|
||
if(false == bFindLineNum) {
|
||
laserLines.push_back(std::make_pair(eDataType, sLaserData));
|
||
}
|
||
|
||
int lineIndex;
|
||
unsigned int timeStamp;
|
||
int ptNum = 0;
|
||
sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
|
||
|
||
sLaserData.llFrameIdx = lineIndex;
|
||
sLaserData.llTimeStamp = timeStamp;
|
||
sLaserData.nPointCount = ptNum;
|
||
if (eDataType == keResultDataType_PointXYZRGBA) {
|
||
sLaserData.p3DPoint = new SVzNLPointXYZRGBA[ptNum];
|
||
sLaserData.p2DPoint = new SVzNL2DLRPoint[ptNum];
|
||
memset(sLaserData.p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * ptNum);
|
||
memset(sLaserData.p2DPoint, 0, sizeof(SVzNL2DLRPoint) * ptNum);
|
||
} else if(eDataType == keResultDataType_Position) {
|
||
sLaserData.p3DPoint = new SVzNL3DPosition[ptNum];
|
||
sLaserData.p2DPoint = new SVzNL2DPosition[ptNum];
|
||
memset(sLaserData.p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
|
||
memset(sLaserData.p2DPoint, 0, sizeof(SVzNL2DPosition) * ptNum);
|
||
}
|
||
nLaserPointIdx = 0;
|
||
bFindLineNum = false;
|
||
|
||
} else if (line.find("Poly_") == 0) {
|
||
// 跳过 Poly 折线头及后续点坐标数据
|
||
size_t lastUnderscore = line.rfind('_');
|
||
int polyNum = 0;
|
||
if (lastUnderscore != std::string::npos && lastUnderscore + 1 < line.size()) {
|
||
polyNum = std::atoi(line.c_str() + lastUnderscore + 1);
|
||
}
|
||
for (int skip = 0; skip < polyNum; ) {
|
||
if (!std::getline(inputFile, line)) break;
|
||
TrimCarriageReturn(line);
|
||
if (!line.empty()) {
|
||
++skip;
|
||
}
|
||
}
|
||
|
||
} else if (line.find("{") == 0) {
|
||
// 使用正则表达式判断是XYZ还是RGBD格式
|
||
// XYZ格式: {x,y,z}-{leftX,leftY}-{rightX,rightY}
|
||
// RGBD格式: {x,y,z,r,g,b}-{leftX,leftY}-{rightX,rightY}
|
||
|
||
// 更精确的正则表达式，匹配完整的行格式
|
||
if(sLaserData.p3DPoint == nullptr || sLaserData.p2DPoint == nullptr) {
|
||
LOG_ERROR("sLaserData.p3DPoint == nullptr || sLaserData.p2DPoint == nullptr \n");
|
||
return ERR_CODE(DATA_ERR_INVALID);
|
||
}
|
||
|
||
if (eDataType == keResultDataType_PointXYZRGBA) {
|
||
SVzNLPointXYZRGBA* pRGBAPoints = static_cast<SVzNLPointXYZRGBA*>(sLaserData.p3DPoint);
|
||
SVzNL2DLRPoint* p2DPoints = static_cast<SVzNL2DLRPoint*>(sLaserData.p2DPoint);
|
||
_ParseLaserScanPoint(line, pRGBAPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
|
||
nLaserPointIdx++;
|
||
} else {
|
||
SVzNL3DPosition* p3DPoints = static_cast<SVzNL3DPosition*>(sLaserData.p3DPoint);
|
||
SVzNL2DPosition* p2DPoints = static_cast<SVzNL2DPosition*>(sLaserData.p2DPoint);
|
||
_ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
|
||
p3DPoints[nLaserPointIdx].nPointIdx = nLaserPointIdx;
|
||
nLaserPointIdx++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 添加最后一条扫描线数据
|
||
if (!bFindLineNum) {
|
||
laserLines.push_back(std::make_pair(eDataType, sLaserData));
|
||
}
|
||
|
||
inputFile.close();
|
||
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<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
||
int lineNum,
|
||
float scanSpeed,
|
||
int maxTimeStamp,
|
||
int clockPerSecond)
|
||
{
|
||
LOG_INFO("Saving unified laser scan data to file: %s\n", fileName.c_str());
|
||
|
||
if (laserLines.empty() || lineNum <= 0) {
|
||
m_lastError = "Invalid input parameters for saving unified data";
|
||
LOG_ERROR("Invalid parameters for saving unified 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 (const auto& linePair : laserLines) {
|
||
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
sw << "Line_" << lineData.llFrameIdx << "_" << lineData.llTimeStamp << "_" << lineData.nPointCount << std::endl;
|
||
|
||
// 根据数据类型写入点云数据
|
||
if (dataType == keResultDataType_Position && lineData.p3DPoint) {
|
||
// 写入XYZ格式数据
|
||
const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
|
||
const SVzNL2DPosition* points2D = static_cast<const SVzNL2DPosition*>(lineData.p2DPoint);
|
||
for (int i = 0; i < lineData.nPointCount; i++) {
|
||
float x = static_cast<float>(points[i].pt3D.x);
|
||
float y = static_cast<float>(points[i].pt3D.y);
|
||
float z = static_cast<float>(points[i].pt3D.z);
|
||
sw << "{ " << std::fixed << std::setprecision(6) << x << "," << y << "," << z << " }-";
|
||
sw << "{ " << points2D[i].ptLeft2D.x << "," << points2D[i].ptLeft2D.y << " }-";
|
||
sw << "{ " << points2D[i].ptRight2D.x << "," << points2D[i].ptRight2D.y << " }" << std::endl;
|
||
}
|
||
} else if (dataType == keResultDataType_PointXYZRGBA && lineData.p3DPoint) {
|
||
// 写入RGBD格式数据
|
||
const SVzNLPointXYZRGBA* points = static_cast<const SVzNLPointXYZRGBA*>(lineData.p3DPoint);
|
||
const SVzNL2DLRPoint* points2D = static_cast<const SVzNL2DLRPoint*>(lineData.p2DPoint);
|
||
for (int i = 0; i < lineData.nPointCount; i++) {
|
||
float x = static_cast<float>(points[i].x);
|
||
float y = static_cast<float>(points[i].y);
|
||
float z = static_cast<float>(points[i].z);
|
||
int r = (points[i].nRGB >> 16) & 0xFF;
|
||
int g = (points[i].nRGB >> 8) & 0xFF;
|
||
int b = points[i].nRGB & 0xFF;
|
||
|
||
sw << "{" << std::fixed << std::setprecision(6) << x << ","
|
||
<< std::fixed << std::setprecision(6) << y << ","
|
||
<< std::fixed << std::setprecision(6) << z << ","
|
||
<< std::fixed << std::setprecision(6) << b * 1.0f / 255 << ","
|
||
<< std::fixed << std::setprecision(6) << g * 1.0f / 255 << ","
|
||
<< std::fixed << std::setprecision(6) << r * 1.0f / 255 << "}-";
|
||
sw << "{ " << points2D[i].sLeft.x << "," << points2D[i].sLeft.y << " }-";
|
||
sw << "{ " << points2D[i].sRight.x << "," << points2D[i].sRight.y << " }" << std::endl;
|
||
}
|
||
}
|
||
}
|
||
|
||
sw.close();
|
||
return SUCCESS;
|
||
|
||
} catch (const std::exception& e) {
|
||
m_lastError = "Error saving unified file: " + std::string(e.what());
|
||
LOG_ERROR("Error saving unified laser data to file: %s\n", e.what());
|
||
return ERR_CODE(FILE_ERR_WRITE);
|
||
}
|
||
}
|
||
|
||
int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition> > xyzData)
|
||
{
|
||
LOG_INFO("Saving unified laser scan data to file: %s\n", fileName.c_str());
|
||
|
||
if (xyzData.empty()) {
|
||
m_lastError = "Invalid input parameters for saving unified data";
|
||
LOG_ERROR("Invalid parameters for saving unified 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:" << xyzData.size() << std::endl;
|
||
sw << "DataType: 0" << std::endl;
|
||
sw << "ScanSpeed:" << 0 << std::endl;
|
||
sw << "PointAdjust: 1" << std::endl;
|
||
sw << "MaxTimeStamp:" << 0 << "_" << 0 << std::endl;
|
||
|
||
int index = 0;
|
||
// 写入每条扫描线数据
|
||
for (const auto& linePair : xyzData) {
|
||
sw << "Line_" << index++ << "_" << 0 << "_" << linePair.size() << std::endl;
|
||
// 根据数据类型写入点云数据
|
||
for(const auto& point : linePair){
|
||
|
||
// 写入XYZ格式数据
|
||
float x = static_cast<float>(point.pt3D.x);
|
||
float y = static_cast<float>(point.pt3D.y);
|
||
float z = static_cast<float>(point.pt3D.z);
|
||
sw << "{ "
|
||
<< std::fixed << std::setprecision(6) << x << ", "
|
||
<< std::fixed << std::setprecision(6) << y << ", "
|
||
<< std::fixed << std::setprecision(6) << z << " } - ";
|
||
sw << "{ 0, 0 } - { 0, 0 }" << std::endl;
|
||
}
|
||
}
|
||
|
||
sw.close();
|
||
return SUCCESS;
|
||
|
||
} catch (const std::exception& e) {
|
||
m_lastError = "Error saving unified file: " + std::string(e.what());
|
||
LOG_ERROR("Error saving unified laser data to file: %s\n", e.what());
|
||
return ERR_CODE(FILE_ERR_WRITE);
|
||
}
|
||
}
|
||
|
||
int LaserDataLoader::LoadPolySegments(const std::string& fileName,
|
||
std::vector<std::vector<SVzNLPointXYZRGBA>>& polyLines)
|
||
{
|
||
LOG_INFO("Loading poly segments from file: %s\n", fileName.c_str());
|
||
|
||
polyLines.clear();
|
||
|
||
std::ifstream inputFile(fileName);
|
||
if (!inputFile.is_open()) {
|
||
m_lastError = "Cannot open file: " + fileName;
|
||
LOG_ERROR("Cannot open file: %s\n", fileName.c_str());
|
||
return ERR_CODE(FILE_ERR_NOEXIST);
|
||
}
|
||
|
||
std::string line;
|
||
|
||
while (std::getline(inputFile, line)) {
|
||
TrimCarriageReturn(line);
|
||
|
||
// 跳过空行和注释
|
||
if (line.empty() || line[0] == '#') {
|
||
continue;
|
||
}
|
||
|
||
// 查找 Poly_index_num 头
|
||
if (line.find("Poly_") != 0) {
|
||
continue;
|
||
}
|
||
|
||
// 解析点数量（最后一个下划线后的数字）
|
||
size_t lastUnderscore = line.rfind('_');
|
||
if (lastUnderscore == std::string::npos || lastUnderscore + 1 >= line.size()) {
|
||
LOG_WARN("[LaserDataLoader] Invalid Poly header: %s\n", line.c_str());
|
||
continue;
|
||
}
|
||
|
||
int num = std::atoi(line.c_str() + lastUnderscore + 1);
|
||
if (num < 2) {
|
||
LOG_WARN("[LaserDataLoader] Invalid point count in Poly header: %s\n", line.c_str());
|
||
continue;
|
||
}
|
||
|
||
// 读取 num 个点坐标
|
||
std::vector<SVzNLPointXYZRGBA> points;
|
||
for (int i = 0; i < num; ) {
|
||
if (!std::getline(inputFile, line)) break;
|
||
TrimCarriageReturn(line);
|
||
if (line.empty()) {
|
||
continue; // 跳过空行，不计数
|
||
}
|
||
|
||
SVzNLPointXYZRGBA pt;
|
||
memset(&pt, 0, sizeof(pt));
|
||
pt.nRGB = 0x00FFFFFF; // 默认白色
|
||
bool parsed = false;
|
||
|
||
// 尝试花括号格式
|
||
if (line.find('{') != std::string::npos) {
|
||
float fx, fy, fz;
|
||
int R = 255, G = 255, B = 255, A = 255;
|
||
|
||
// 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A}
|
||
int cnt = sscanf(line.c_str(),
|
||
" { %f , %f , %f } - { %*f , %*f } - { %*f , %*f } - { %d , %d , %d , %d }",
|
||
&fx, &fy, &fz, &R, &G, &B, &A);
|
||
if (cnt >= 3) {
|
||
pt.x = fx;
|
||
pt.y = fy;
|
||
pt.z = fz;
|
||
if (cnt >= 7) {
|
||
// 解析到了颜色 R,G,B,A（0-255），Alpha 存储在高 8 位
|
||
unsigned int nRGB = static_cast<unsigned int>(A) << 24;
|
||
nRGB |= static_cast<unsigned int>(B) << 16;
|
||
nRGB |= static_cast<unsigned int>(G) << 8;
|
||
nRGB |= static_cast<unsigned int>(R);
|
||
pt.nRGB = nRGB;
|
||
}
|
||
parsed = true;
|
||
}
|
||
|
||
// 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry}
|
||
if (!parsed) {
|
||
float r, g, b;
|
||
if (sscanf(line.c_str(), " { %f , %f , %f , %f , %f , %f }",
|
||
&fx, &fy, &fz, &r, &g, &b) == 6) {
|
||
pt.x = fx;
|
||
pt.y = fy;
|
||
pt.z = fz;
|
||
int nRGB = (int)(b * 255); nRGB <<= 8;
|
||
nRGB += (int)(g * 255); nRGB <<= 8;
|
||
nRGB += (int)(r * 255);
|
||
pt.nRGB = nRGB;
|
||
parsed = true;
|
||
}
|
||
}
|
||
|
||
// XYZ格式: {x,y,z}-{...}-{...}
|
||
if (!parsed) {
|
||
if (sscanf(line.c_str(), " { %f , %f , %f }", &fx, &fy, &fz) == 3) {
|
||
pt.x = fx;
|
||
pt.y = fy;
|
||
pt.z = fz;
|
||
parsed = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 尝试纯数字格式: x y z（空格/Tab/逗号分隔）
|
||
if (!parsed) {
|
||
float fx, fy, fz;
|
||
std::string lineCopy = line;
|
||
for (char& c : lineCopy) {
|
||
if (c == ',' || c == '\t') c = ' ';
|
||
}
|
||
if (sscanf(lineCopy.c_str(), "%f %f %f", &fx, &fy, &fz) == 3) {
|
||
pt.x = fx;
|
||
pt.y = fy;
|
||
pt.z = fz;
|
||
parsed = true;
|
||
}
|
||
}
|
||
|
||
if (parsed) {
|
||
points.push_back(pt);
|
||
}
|
||
++i;
|
||
}
|
||
|
||
if (points.size() >= 2) {
|
||
polyLines.push_back(std::move(points));
|
||
}
|
||
}
|
||
|
||
inputFile.close();
|
||
LOG_INFO("Loaded %zu poly lines from file: %s\n", polyLines.size(), fileName.c_str());
|
||
return SUCCESS;
|
||
}
|
||
|
||
void LaserDataLoader::FreeLaserScanData(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines)
|
||
{
|
||
LOG_DEBUG("Freeing unified laser scan data, line count: %zu\n", laserLines.size());
|
||
|
||
if (!laserLines.empty()) {
|
||
for (auto& linePair : laserLines) {
|
||
EVzResultDataType dataType = linePair.first;
|
||
SVzLaserLineData& lineData = linePair.second;
|
||
|
||
if (lineData.p3DPoint) {
|
||
delete[] lineData.p3DPoint;
|
||
lineData.p3DPoint = nullptr;
|
||
}
|
||
|
||
if (lineData.p2DPoint) {
|
||
delete[] lineData.p2DPoint;
|
||
lineData.p2DPoint = nullptr;
|
||
}
|
||
}
|
||
laserLines.clear();
|
||
}
|
||
|
||
LOG_DEBUG("Unified laser scan data freed successfully\n");
|
||
}
|
||
|
||
// 转换统一格式数据为std::vector<std::vector<SVzNL3DPosition>>格式
|
||
int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
|
||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||
size_t* validPointCount)
|
||
{
|
||
LOG_DEBUG("Converting unified data to scan lines format\n");
|
||
|
||
scanLines.clear();
|
||
|
||
// 初始化有效点计数
|
||
size_t validCount = 0;
|
||
|
||
// 浮点数零值判断阈值
|
||
const double EPSILON = 1e-6;
|
||
|
||
for (const auto& linePair : unifiedData) {
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
// 只处理Position类型的数据
|
||
if (dataType == keResultDataType_Position && lineData.p3DPoint) {
|
||
std::vector<SVzNL3DPosition> scanLine;
|
||
|
||
// 为当前扫描线分配空间
|
||
scanLine.reserve(lineData.nPointCount);
|
||
|
||
// 复制点数据并统计有效点
|
||
const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
|
||
for (int i = 0; i < lineData.nPointCount; i++) {
|
||
scanLine.push_back(points[i]);
|
||
|
||
// 统计有效点（xyz全非0，使用epsilon阈值判断）
|
||
if (std::fabs(points[i].pt3D.x) > EPSILON &&
|
||
std::fabs(points[i].pt3D.y) > EPSILON &&
|
||
std::fabs(points[i].pt3D.z) > EPSILON) {
|
||
validCount++;
|
||
}
|
||
}
|
||
|
||
scanLines.push_back(scanLine);
|
||
}
|
||
}
|
||
|
||
// 返回有效点计数（如果调用者需要）
|
||
if (validPointCount) {
|
||
*validPointCount = validCount;
|
||
}
|
||
|
||
LOG_DEBUG("Converted %zu lines to scan lines format, valid points: %zu\n", scanLines.size(), validCount);
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 转换统一格式数据为SVzNL3DLaserLine格式
|
||
int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
|
||
std::vector<SVzNL3DLaserLine>& xyzData)
|
||
{
|
||
xyzData.clear();
|
||
|
||
for (const auto& linePair : unifiedData) {
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
// 只处理Position类型的数据
|
||
if (dataType == keResultDataType_Position && lineData.p3DPoint) {
|
||
SVzNL3DLaserLine xyzLine;
|
||
xyzLine.nTimeStamp = lineData.llTimeStamp;
|
||
xyzLine.nPositionCnt = lineData.nPointCount;
|
||
|
||
if (lineData.nPointCount > 0) {
|
||
xyzLine.p3DPosition = new SVzNL3DPosition[lineData.nPointCount];
|
||
if (xyzLine.p3DPosition) {
|
||
memcpy(xyzLine.p3DPosition, lineData.p3DPoint, sizeof(SVzNL3DPosition) * lineData.nPointCount);
|
||
} else {
|
||
m_lastError = "Memory allocation failed for SVzNL3DPosition";
|
||
LOG_ERROR("Memory allocation failed for SVzNL3DPosition\n");
|
||
return ERR_CODE(DATA_ERR_INVALID);
|
||
}
|
||
} else {
|
||
xyzLine.p3DPosition = nullptr;
|
||
}
|
||
|
||
xyzData.push_back(xyzLine);
|
||
}
|
||
}
|
||
|
||
LOG_DEBUG("Converted %zu lines to SVzNL3DLaserLine format\n", xyzData.size());
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 转换统一格式数据为SVzNLXYZRGBDLaserLine格式
|
||
int LaserDataLoader::ConvertToSVzNLXYZRGBDLaserLine(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
|
||
std::vector<SVzNLXYZRGBDLaserLine>& rgbdData)
|
||
{
|
||
LOG_DEBUG("Converting unified data to SVzNLXYZRGBDLaserLine format\n");
|
||
|
||
rgbdData.clear();
|
||
|
||
for (const auto& linePair : unifiedData) {
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
// 只处理PointXYZRGBA类型的数据
|
||
if (dataType == keResultDataType_PointXYZRGBA && lineData.p3DPoint) {
|
||
SVzNLXYZRGBDLaserLine rgbdLine;
|
||
rgbdLine.nTimeStamp = lineData.llTimeStamp;
|
||
rgbdLine.nPointCnt = lineData.nPointCount;
|
||
|
||
if (lineData.nPointCount > 0) {
|
||
rgbdLine.p3DPoint = new SVzNLPointXYZRGBA[lineData.nPointCount];
|
||
if (rgbdLine.p3DPoint) {
|
||
memcpy(rgbdLine.p3DPoint, lineData.p3DPoint, sizeof(SVzNLPointXYZRGBA) * lineData.nPointCount);
|
||
} else {
|
||
m_lastError = "Memory allocation failed for SVzNLPointXYZRGBA";
|
||
LOG_ERROR("Memory allocation failed for SVzNLPointXYZRGBA\n");
|
||
return ERR_CODE(DATA_ERR_INVALID);
|
||
}
|
||
} else {
|
||
rgbdLine.p3DPoint = nullptr;
|
||
}
|
||
|
||
rgbdData.push_back(rgbdLine);
|
||
}
|
||
}
|
||
|
||
LOG_DEBUG("Converted %zu lines to SVzNLXYZRGBDLaserLine format\n", rgbdData.size());
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 释放转换后的SVzNL3DLaserLine数据内存
|
||
void LaserDataLoader::FreeConvertedData(std::vector<SVzNL3DLaserLine>& xyzData)
|
||
{
|
||
LOG_DEBUG("Freeing converted SVzNL3DLaserLine data, line count: %zu\n", xyzData.size());
|
||
|
||
if (!xyzData.empty()) {
|
||
for (auto& scanLine : xyzData) {
|
||
if (scanLine.p3DPosition) {
|
||
delete[] scanLine.p3DPosition;
|
||
scanLine.p3DPosition = nullptr;
|
||
}
|
||
}
|
||
xyzData.clear();
|
||
}
|
||
|
||
LOG_DEBUG("Converted SVzNL3DLaserLine data freed successfully\n");
|
||
}
|
||
|
||
// 释放转换后的SVzNLXYZRGBDLaserLine数据内存
|
||
void LaserDataLoader::FreeConvertedData(std::vector<SVzNLXYZRGBDLaserLine>& rgbdData)
|
||
{
|
||
LOG_DEBUG("Freeing converted SVzNLXYZRGBDLaserLine data, line count: %zu\n", rgbdData.size());
|
||
|
||
if (!rgbdData.empty()) {
|
||
for (auto& scanLine : rgbdData) {
|
||
if (scanLine.p3DPoint) {
|
||
delete[] scanLine.p3DPoint;
|
||
scanLine.p3DPoint = nullptr;
|
||
}
|
||
}
|
||
rgbdData.clear();
|
||
}
|
||
|
||
LOG_DEBUG("Converted SVzNLXYZRGBDLaserLine data freed successfully\n");
|
||
}
|
||
|
||
|
||
int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPosition& s2DData)
|
||
{
|
||
float X, Y, Z;
|
||
float leftX, leftY;
|
||
float rightX, rightY;
|
||
sscanf(data.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||
sData.pt3D.x = X;
|
||
sData.pt3D.y = Y;
|
||
sData.pt3D.z = Z;
|
||
s2DData.ptLeft2D.x = leftX;
|
||
s2DData.ptLeft2D.y = leftY;
|
||
s2DData.ptRight2D.x = rightX;
|
||
s2DData.ptRight2D.y = rightY;
|
||
return SUCCESS;
|
||
}
|
||
|
||
int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNLPointXYZRGBA& sData, SVzNL2DLRPoint& s2DData)
|
||
{
|
||
float X, Y, Z;
|
||
float leftX, leftY;
|
||
float rightX, rightY;
|
||
|
||
// 尝试新格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A}
|
||
int R, G, B, A;
|
||
int parsed = sscanf(data.c_str(),
|
||
" { %f , %f , %f } - { %f , %f } - { %f , %f } - { %d , %d , %d , %d }",
|
||
&X, &Y, &Z, &leftX, &leftY, &rightX, &rightY, &R, &G, &B, &A);
|
||
if (parsed >= 11) {
|
||
sData.x = X;
|
||
sData.y = Y;
|
||
sData.z = Z;
|
||
|
||
// R,G,B,A 是 0-255 整数值，Alpha 存储在高 8 位
|
||
unsigned int nRGB = static_cast<unsigned int>(A) << 24;
|
||
nRGB |= static_cast<unsigned int>(B) << 16;
|
||
nRGB |= static_cast<unsigned int>(G) << 8;
|
||
nRGB |= static_cast<unsigned int>(R);
|
||
sData.nRGB = nRGB;
|
||
|
||
s2DData.sLeft.x = leftX;
|
||
s2DData.sLeft.y = leftY;
|
||
s2DData.sRight.x = rightX;
|
||
s2DData.sRight.y = rightY;
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 旧格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry} r,g,b 为 0~1 浮点值
|
||
float r, g, b;
|
||
sscanf(data.c_str(),
|
||
" { %f , %f , %f , %f , %f , %f } - { %f , %f } - { %f , %f }",
|
||
&X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY);
|
||
sData.x = X;
|
||
sData.y = Y;
|
||
sData.z = Z;
|
||
|
||
int nr = (int)(r * 255);
|
||
int ng = (int)(g * 255);
|
||
int nb = (int)(b * 255);
|
||
nb <<= 8;
|
||
nb += ng;
|
||
nb <<= 8;
|
||
nb += nr;
|
||
sData.nRGB = nb;
|
||
|
||
s2DData.sLeft.x = leftX;
|
||
s2DData.sLeft.y = leftY;
|
||
s2DData.sRight.x = rightX;
|
||
s2DData.sRight.y = rightY;
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 获取激光数据类型
|
||
int LaserDataLoader::_GetLaserType(const std::string& fileName, EVzResultDataType& eDataType)
|
||
{
|
||
std::ifstream inputFile(fileName);
|
||
std::string linedata;
|
||
|
||
if (!inputFile.is_open()) {
|
||
m_lastError = "Cannot open file: " + fileName;
|
||
return ERR_CODE(FILE_ERR_NOEXIST);
|
||
}
|
||
|
||
bool bFind = false;
|
||
while (std::getline(inputFile, linedata)) {
|
||
TrimCarriageReturn(linedata);
|
||
|
||
if (linedata.find("{") == 0) {
|
||
// 统计 { 的数量判断格式
|
||
int braceCount = 0;
|
||
for (char c : linedata) {
|
||
if (c == '{') braceCount++;
|
||
}
|
||
|
||
if (braceCount == 4) {
|
||
// 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A}
|
||
eDataType = keResultDataType_PointXYZRGBA;
|
||
bFind = true;
|
||
} else if (braceCount == 3) {
|
||
// 检查第一组的逗号数量区分XYZ和旧RGBD
|
||
size_t firstClose = linedata.find('}');
|
||
if (firstClose != std::string::npos) {
|
||
int commaCount = 0;
|
||
for (size_t i = 0; i < firstClose; ++i) {
|
||
if (linedata[i] == ',') commaCount++;
|
||
}
|
||
if (commaCount >= 5) {
|
||
// 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry}
|
||
eDataType = keResultDataType_PointXYZRGBA;
|
||
bFind = true;
|
||
} else if (commaCount >= 2) {
|
||
// XYZ格式: {x,y,z}-{lx,ly}-{rx,ry}
|
||
eDataType = keResultDataType_Position;
|
||
bFind = true;
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
inputFile.close();
|
||
return bFind ? SUCCESS : ERR_CODE(FILE_ERR_FORMAT);
|
||
}
|