1167 lines
44 KiB
C++
1167 lines
44 KiB
C++
#include "LaserDataLoader.h"
|
||
#include <algorithm>
|
||
#include <cctype>
|
||
#include <cstdarg>
|
||
#include <cstdio>
|
||
#include <cstdint>
|
||
#include <cstring>
|
||
#include <cstdlib>
|
||
#include <cmath>
|
||
#include <fstream>
|
||
#include <iomanip>
|
||
#include <memory>
|
||
#include <new>
|
||
#include <sstream>
|
||
#include <stdexcept>
|
||
#include <regex>
|
||
|
||
#include "VrLog.h"
|
||
|
||
// 辅助函数：去除字符串末尾的 \r 字符（处理 Windows 格式的 CR LF 换行符）
|
||
static inline void TrimCarriageReturn(std::string& str)
|
||
{
|
||
if (!str.empty() && str.back() == '\r') {
|
||
str.pop_back();
|
||
}
|
||
}
|
||
|
||
namespace
|
||
{
|
||
struct DatValue2DFloat
|
||
{
|
||
double x;
|
||
double y;
|
||
double value;
|
||
};
|
||
|
||
struct Dat3DPoint
|
||
{
|
||
double X;
|
||
double Y;
|
||
double Z;
|
||
};
|
||
|
||
struct Dat3DFeaturePoint
|
||
{
|
||
int index;
|
||
int flag;
|
||
DatValue2DFloat stereo;
|
||
DatValue2DFloat stereoRight;
|
||
Dat3DPoint eye;
|
||
Dat3DPoint world;
|
||
std::uint64_t next;
|
||
};
|
||
|
||
struct DatLaserScan3DLinesHeader
|
||
{
|
||
char dataCalibrated;
|
||
char adjusted;
|
||
int scanLineNum;
|
||
int vr1;
|
||
int vr2;
|
||
double lineV;
|
||
int clock;
|
||
std::uint32_t maxTimeStamp;
|
||
std::uint64_t scanLineList;
|
||
};
|
||
|
||
struct DatLineScan3DData
|
||
{
|
||
int lineIndex;
|
||
std::uint32_t timeStamp;
|
||
double lineStepping;
|
||
};
|
||
|
||
static_assert(sizeof(DatLaserScan3DLinesHeader) == 40, "Unexpected dat header layout");
|
||
static_assert(sizeof(Dat3DFeaturePoint) == 112, "Unexpected dat feature point layout");
|
||
|
||
template <typename T>
|
||
bool ReadBinary(std::ifstream& inputFile, T& value)
|
||
{
|
||
inputFile.read(reinterpret_cast<char*>(&value), sizeof(T));
|
||
return static_cast<bool>(inputFile);
|
||
}
|
||
|
||
bool HasFileExtension(const std::string& fileName, const char* extension)
|
||
{
|
||
const size_t dotPos = fileName.find_last_of('.');
|
||
if (dotPos == std::string::npos) {
|
||
return false;
|
||
}
|
||
|
||
const size_t slashPos = fileName.find_last_of("/\\");
|
||
if (slashPos != std::string::npos && dotPos < slashPos) {
|
||
return false;
|
||
}
|
||
|
||
std::string fileExt = fileName.substr(dotPos);
|
||
std::transform(fileExt.begin(), fileExt.end(), fileExt.begin(),
|
||
[](unsigned char c) { return static_cast<char>(std::tolower(c)); });
|
||
return fileExt == extension;
|
||
}
|
||
|
||
bool LooksLikeTextLaserFile(const std::string& fileName)
|
||
{
|
||
std::ifstream inputFile(fileName, std::ios::binary);
|
||
if (!inputFile.is_open()) {
|
||
return false;
|
||
}
|
||
|
||
char header[8] = {};
|
||
inputFile.read(header, sizeof(header));
|
||
return inputFile.gcount() == sizeof(header) && std::memcmp(header, "LineNum:", sizeof(header)) == 0;
|
||
}
|
||
|
||
bool ReadDatLineRecord(std::ifstream& inputFile, int recordSize, DatLineScan3DData& lineData)
|
||
{
|
||
char buffer[40] = {};
|
||
if (recordSize <= 0 || recordSize > static_cast<int>(sizeof(buffer))) {
|
||
return false;
|
||
}
|
||
|
||
inputFile.read(buffer, recordSize);
|
||
if (!inputFile) {
|
||
return false;
|
||
}
|
||
|
||
std::memcpy(&lineData.lineIndex, buffer, sizeof(lineData.lineIndex));
|
||
std::memcpy(&lineData.timeStamp, buffer + 4, sizeof(lineData.timeStamp));
|
||
std::memcpy(&lineData.lineStepping, buffer + 8, sizeof(lineData.lineStepping));
|
||
return true;
|
||
}
|
||
|
||
void AppendFormattedLine(std::string& output, const char* format, ...)
|
||
{
|
||
char stackBuffer[256];
|
||
|
||
va_list args;
|
||
va_start(args, format);
|
||
va_list argsCopy;
|
||
va_copy(argsCopy, args);
|
||
int written = vsnprintf(stackBuffer, sizeof(stackBuffer), format, args);
|
||
va_end(args);
|
||
|
||
if (written < 0) {
|
||
va_end(argsCopy);
|
||
return;
|
||
}
|
||
|
||
if (static_cast<size_t>(written) < sizeof(stackBuffer)) {
|
||
output.append(stackBuffer, static_cast<size_t>(written));
|
||
va_end(argsCopy);
|
||
return;
|
||
}
|
||
|
||
std::vector<char> dynamicBuffer(static_cast<size_t>(written) + 1);
|
||
vsnprintf(dynamicBuffer.data(), dynamicBuffer.size(), format, argsCopy);
|
||
va_end(argsCopy);
|
||
output.append(dynamicBuffer.data(), static_cast<size_t>(written));
|
||
}
|
||
}
|
||
|
||
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 from file: %s\n", fileName.c_str());
|
||
|
||
// 清空输出参数
|
||
laserLines.clear();
|
||
lineNum = 0;
|
||
scanSpeed = 0.0f;
|
||
maxTimeStamp = 0;
|
||
clockPerSecond = 0;
|
||
|
||
if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) {
|
||
return _LoadLaserScanDataFromDatFile(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
|
||
}
|
||
|
||
// 判断文件类型
|
||
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 (nLaserPointIdx >= sLaserData.nPointCount) {
|
||
LOG_WARN("nLaserPointIdx(%d) >= nPointCount(%d), skip\n", nLaserPointIdx, sLaserData.nPointCount);
|
||
continue;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
// Read binary .dat laser scan data.
|
||
int LaserDataLoader::_LoadLaserScanDataFromDatFile(const std::string& fileName,
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
|
||
int& lineNum,
|
||
float& scanSpeed,
|
||
int& maxTimeStamp,
|
||
int& clockPerSecond)
|
||
{
|
||
std::ifstream inputFile(fileName, std::ios::binary);
|
||
if (!inputFile.is_open()) {
|
||
m_lastError = "Cannot open file: " + fileName;
|
||
LOG_ERROR("Cannot open dat file: %s\n", fileName.c_str());
|
||
return ERR_CODE(FILE_ERR_NOEXIST);
|
||
}
|
||
|
||
auto fail = [&](int code, const std::string& message) -> int {
|
||
m_lastError = message;
|
||
LOG_ERROR("%s\n", message.c_str());
|
||
FreeLaserScanData(laserLines);
|
||
lineNum = 0;
|
||
scanSpeed = 0.0f;
|
||
maxTimeStamp = 0;
|
||
clockPerSecond = 0;
|
||
return ERR_CODE(code);
|
||
};
|
||
|
||
inputFile.seekg(0, std::ios::end);
|
||
const std::streamoff fileSize = inputFile.tellg();
|
||
inputFile.seekg(0, std::ios::beg);
|
||
|
||
DatLaserScan3DLinesHeader header;
|
||
std::memset(&header, 0, sizeof(header));
|
||
if (!ReadBinary(inputFile, header)) {
|
||
return fail(FILE_ERR_READ, "Cannot read dat header: " + fileName);
|
||
}
|
||
|
||
if (header.scanLineNum < 0 || fileSize < static_cast<std::streamoff>(sizeof(header))) {
|
||
return fail(FILE_ERR_FORMAT, "Invalid dat header: " + fileName);
|
||
}
|
||
|
||
const int kFeaturePointSize = static_cast<int>(sizeof(Dat3DFeaturePoint));
|
||
auto validateLayout = [&](int lineRecordSize) -> bool {
|
||
std::streamoff pos = static_cast<std::streamoff>(sizeof(header));
|
||
inputFile.clear();
|
||
inputFile.seekg(pos, std::ios::beg);
|
||
|
||
for (int i = 0; i < header.scanLineNum; ++i) {
|
||
if (pos + lineRecordSize + static_cast<int>(sizeof(int)) > fileSize) {
|
||
return false;
|
||
}
|
||
|
||
inputFile.seekg(lineRecordSize, std::ios::cur);
|
||
pos += lineRecordSize;
|
||
|
||
int pointCount = 0;
|
||
if (!ReadBinary(inputFile, pointCount)) {
|
||
return false;
|
||
}
|
||
pos += static_cast<std::streamoff>(sizeof(pointCount));
|
||
|
||
if (pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
|
||
return false;
|
||
}
|
||
|
||
const std::streamoff pointBytes =
|
||
static_cast<std::streamoff>(pointCount) * static_cast<std::streamoff>(kFeaturePointSize);
|
||
if (pointBytes < 0 || pos + pointBytes > fileSize) {
|
||
return false;
|
||
}
|
||
|
||
inputFile.seekg(pointBytes, std::ios::cur);
|
||
pos += pointBytes;
|
||
}
|
||
|
||
return pos == fileSize;
|
||
};
|
||
|
||
int lineRecordSize = 40;
|
||
if (!validateLayout(lineRecordSize)) {
|
||
lineRecordSize = 32;
|
||
if (!validateLayout(lineRecordSize)) {
|
||
return fail(FILE_ERR_FORMAT, "Invalid dat layout: " + fileName);
|
||
}
|
||
}
|
||
|
||
lineNum = header.scanLineNum;
|
||
scanSpeed = static_cast<float>(header.lineV);
|
||
maxTimeStamp = static_cast<int>(header.maxTimeStamp);
|
||
clockPerSecond = header.clock;
|
||
|
||
laserLines.clear();
|
||
laserLines.reserve(static_cast<size_t>(lineNum));
|
||
|
||
inputFile.clear();
|
||
inputFile.seekg(static_cast<std::streamoff>(sizeof(header)), std::ios::beg);
|
||
|
||
for (int i = 0; i < lineNum; ++i) {
|
||
DatLineScan3DData datLine;
|
||
std::memset(&datLine, 0, sizeof(datLine));
|
||
if (!ReadDatLineRecord(inputFile, lineRecordSize, datLine)) {
|
||
return fail(FILE_ERR_READ, "Cannot read dat line header: " + fileName);
|
||
}
|
||
|
||
int pointCount = 0;
|
||
if (!ReadBinary(inputFile, pointCount)) {
|
||
return fail(FILE_ERR_READ, "Cannot read dat point count: " + fileName);
|
||
}
|
||
if (pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
|
||
return fail(FILE_ERR_FORMAT, "Invalid dat point count: " + fileName);
|
||
}
|
||
|
||
SVzLaserLineData lineData;
|
||
std::memset(&lineData, 0, sizeof(lineData));
|
||
lineData.nPointCount = pointCount;
|
||
lineData.dTotleOffset = datLine.lineStepping;
|
||
lineData.dStep = datLine.lineStepping;
|
||
lineData.llFrameIdx = static_cast<unsigned long long>(datLine.lineIndex);
|
||
lineData.llTimeStamp = static_cast<unsigned long long>(datLine.timeStamp);
|
||
|
||
std::unique_ptr<SVzNL3DPosition[]> points3D;
|
||
std::unique_ptr<SVzNL2DPosition[]> points2D;
|
||
|
||
if (pointCount > 0) {
|
||
points3D.reset(new (std::nothrow) SVzNL3DPosition[pointCount]);
|
||
points2D.reset(new (std::nothrow) SVzNL2DPosition[pointCount]);
|
||
if (!points3D || !points2D) {
|
||
return fail(DATA_ERR_MEM, "Memory allocation failed while reading dat: " + fileName);
|
||
}
|
||
|
||
std::memset(points3D.get(), 0, sizeof(SVzNL3DPosition) * static_cast<size_t>(pointCount));
|
||
std::memset(points2D.get(), 0, sizeof(SVzNL2DPosition) * static_cast<size_t>(pointCount));
|
||
|
||
for (int j = 0; j < pointCount; ++j) {
|
||
Dat3DFeaturePoint featurePoint;
|
||
std::memset(&featurePoint, 0, sizeof(featurePoint));
|
||
if (!ReadBinary(inputFile, featurePoint)) {
|
||
return fail(FILE_ERR_READ, "Cannot read dat feature point: " + fileName);
|
||
}
|
||
|
||
points3D[j].nPointIdx = j;
|
||
points3D[j].pt3D.x = featurePoint.eye.X;
|
||
points3D[j].pt3D.y = featurePoint.eye.Y;
|
||
points3D[j].pt3D.z = featurePoint.eye.Z;
|
||
|
||
points2D[j].nPointIdx = j;
|
||
points2D[j].ptLeft2D.x = static_cast<int>(featurePoint.stereo.x);
|
||
points2D[j].ptLeft2D.y = static_cast<int>(featurePoint.stereo.y);
|
||
points2D[j].ptRight2D.x = static_cast<int>(featurePoint.stereoRight.x);
|
||
points2D[j].ptRight2D.y = static_cast<int>(featurePoint.stereoRight.y);
|
||
}
|
||
|
||
lineData.p3DPoint = points3D.get();
|
||
lineData.p2DPoint = points2D.get();
|
||
}
|
||
|
||
laserLines.push_back(std::make_pair(keResultDataType_Position, lineData));
|
||
points3D.release();
|
||
points2D.release();
|
||
}
|
||
|
||
LOG_INFO("Successfully loaded %d dat 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 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 << '\n';
|
||
sw << "DataType: 0" << '\n';
|
||
sw << "ScanSpeed:" << scanSpeed << '\n';
|
||
sw << "PointAdjust: 1" << '\n';
|
||
sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << '\n';
|
||
|
||
// 写入每条扫描线数据
|
||
for (const auto& linePair : laserLines) {
|
||
|
||
EVzResultDataType dataType = linePair.first;
|
||
const SVzLaserLineData& lineData = linePair.second;
|
||
|
||
sw << "Line_" << lineData.llFrameIdx << "_" << lineData.llTimeStamp << "_" << lineData.nPointCount << '\n';
|
||
|
||
// 根据数据类型写入点云数据
|
||
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);
|
||
constexpr int BATCH = 256;
|
||
constexpr int PER_POINT = 128;
|
||
for (int base = 0; base < lineData.nPointCount; base += BATCH) {
|
||
int end = (base + BATCH < lineData.nPointCount) ? (base + BATCH) : lineData.nPointCount;
|
||
std::string buffer;
|
||
buffer.reserve(BATCH * PER_POINT);
|
||
for (int i = base; i < end; ++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);
|
||
AppendFormattedLine(buffer, "{ %.6f,%.6f,%.6f }-{ %d,%d }-{ %d,%d }\n",
|
||
x, y, z,
|
||
points2D[i].ptLeft2D.x, points2D[i].ptLeft2D.y,
|
||
points2D[i].ptRight2D.x, points2D[i].ptRight2D.y);
|
||
}
|
||
sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
|
||
}
|
||
} else if (dataType == keResultDataType_PointXYZI && lineData.p3DPoint) {
|
||
// LiDAR 点云：snprintf 批量写入，每 256 点 flush
|
||
// 坐标 mm 范围可达 ±200000 → "%.6f" 最长 14 字符 → 整行 ≤73 字符
|
||
const SVzNLPointXYZI* points = static_cast<const SVzNLPointXYZI*>(lineData.p3DPoint);
|
||
const int n = lineData.nPointCount;
|
||
constexpr int BATCH = 256;
|
||
constexpr int PER_POINT = 80;
|
||
char buf[BATCH * PER_POINT];
|
||
for (int base = 0; base < n; base += BATCH)
|
||
{
|
||
int end = (base + BATCH < n) ? (base + BATCH) : n;
|
||
char* p = buf;
|
||
for (int i = base; i < end; ++i)
|
||
{
|
||
p += snprintf(p, PER_POINT, "{ %.6f, %.6f, %.6f } - { 0, 0 } - { 0, 0 }\n",
|
||
points[i].fData[0], points[i].fData[1], points[i].fData[2]);
|
||
}
|
||
sw.write(buf, p - buf);
|
||
}
|
||
} 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);
|
||
constexpr int BATCH = 256;
|
||
constexpr int PER_POINT = 160;
|
||
for (int base = 0; base < lineData.nPointCount; base += BATCH) {
|
||
int end = (base + BATCH < lineData.nPointCount) ? (base + BATCH) : lineData.nPointCount;
|
||
std::string buffer;
|
||
buffer.reserve(BATCH * PER_POINT);
|
||
for (int i = base; i < end; ++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;
|
||
|
||
AppendFormattedLine(buffer, "{%.6f,%.6f,%.6f,%.6f,%.6f,%.6f}-{ %d,%d }-{ %d,%d }\n",
|
||
x, y, z,
|
||
b * 1.0f / 255, g * 1.0f / 255, r * 1.0f / 255,
|
||
points2D[i].sLeft.x, points2D[i].sLeft.y,
|
||
points2D[i].sRight.x, points2D[i].sRight.y);
|
||
}
|
||
sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
|
||
}
|
||
}
|
||
}
|
||
|
||
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)
|
||
{
|
||
return DebugSaveLaser(std::move(fileName), std::move(xyzData), nullptr);
|
||
}
|
||
|
||
int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition>> xyzData,
|
||
ISaveProgressCallback* callback)
|
||
{
|
||
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() << '\n';
|
||
sw << "DataType: 0" << '\n';
|
||
sw << "ScanSpeed:" << 0 << '\n';
|
||
sw << "PointAdjust: 1" << '\n';
|
||
sw << "MaxTimeStamp:" << 0 << "_" << 0 << '\n';
|
||
|
||
const size_t totalLines = xyzData.size();
|
||
int index = 0;
|
||
// 写入每条扫描线数据
|
||
for (const auto& linePair : xyzData) {
|
||
if (callback && callback->isSaveCancelled()) {
|
||
sw.close();
|
||
m_lastError = "Save cancelled by user";
|
||
LOG_INFO("Save cancelled by user at line %d/%zu\n", index, totalLines);
|
||
return ERR_CODE(DEV_ARG_INVAILD);
|
||
}
|
||
|
||
sw << "Line_" << index << "_" << 0 << "_" << linePair.size() << '\n';
|
||
// 根据数据类型写入点云数据
|
||
constexpr int BATCH = 256;
|
||
constexpr int PER_POINT = 96;
|
||
for (size_t base = 0; base < linePair.size(); base += BATCH) {
|
||
size_t end = std::min(base + static_cast<size_t>(BATCH), linePair.size());
|
||
std::string buffer;
|
||
buffer.reserve(BATCH * PER_POINT);
|
||
for (size_t i = base; i < end; ++i) {
|
||
const auto& point = linePair[i];
|
||
|
||
// 写入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);
|
||
AppendFormattedLine(buffer, "{ %.6f, %.6f, %.6f } - { 0, 0 } - { 0, 0 }\n",
|
||
x, y, z);
|
||
}
|
||
sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
|
||
}
|
||
|
||
++index;
|
||
if (callback) {
|
||
callback->onSaveProgress(static_cast<float>(index) / static_cast<float>(totalLines));
|
||
}
|
||
}
|
||
|
||
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) {
|
||
switch (dataType) {
|
||
case keResultDataType_Position:
|
||
delete[] static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
|
||
break;
|
||
case keResultDataType_PositionF:
|
||
delete[] static_cast<SVzNL3DPositionF*>(lineData.p3DPoint);
|
||
break;
|
||
case keResultDataType_PointF:
|
||
delete[] static_cast<SVzNL3DPointF*>(lineData.p3DPoint);
|
||
break;
|
||
case keResultDataType_PointXYZ:
|
||
delete[] static_cast<SVzNLPointXYZ*>(lineData.p3DPoint);
|
||
break;
|
||
case keResultDataType_PointXYZRGBA:
|
||
delete[] static_cast<SVzNLPointXYZRGBA*>(lineData.p3DPoint);
|
||
break;
|
||
case keResultDataType_PointGray:
|
||
delete[] static_cast<SVzNLPointXYZGray*>(lineData.p3DPoint);
|
||
break;
|
||
case keResultDataType_PointXYZI:
|
||
delete[] static_cast<SVzNLPointXYZI*>(lineData.p3DPoint);
|
||
break;
|
||
case keResultDataType_PointRGBA_D:
|
||
delete[] static_cast<SVzNLPointRGBA_D*>(lineData.p3DPoint);
|
||
break;
|
||
default:
|
||
LOG_WARN("Unknown laser data type when freeing p3DPoint: %d\n", dataType);
|
||
break;
|
||
}
|
||
lineData.p3DPoint = nullptr;
|
||
}
|
||
|
||
if (lineData.p2DPoint) {
|
||
switch (dataType) {
|
||
case keResultDataType_Position:
|
||
delete[] static_cast<SVzNL2DPosition*>(lineData.p2DPoint);
|
||
break;
|
||
case keResultDataType_PositionF:
|
||
delete[] static_cast<SVzNL2DPositionF*>(lineData.p2DPoint);
|
||
break;
|
||
case keResultDataType_PointF:
|
||
case keResultDataType_PointXYZ:
|
||
case keResultDataType_PointXYZRGBA:
|
||
case keResultDataType_PointGray:
|
||
case keResultDataType_PointXYZI:
|
||
case keResultDataType_PointRGBA_D:
|
||
delete[] static_cast<SVzNL2DLRPoint*>(lineData.p2DPoint);
|
||
break;
|
||
default:
|
||
LOG_WARN("Unknown laser data type when freeing p2DPoint: %d\n", dataType);
|
||
break;
|
||
}
|
||
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);
|
||
}
|