diff --git a/CloudUtils/Inc/LaserDataLoader.h b/CloudUtils/Inc/LaserDataLoader.h index dd692bf..24cee2f 100644 --- a/CloudUtils/Inc/LaserDataLoader.h +++ b/CloudUtils/Inc/LaserDataLoader.h @@ -63,23 +63,28 @@ public: // 转换统一格式数据为std::vector>格式 // validPointCount: 可选输出参数,返回有效点个数(xyz全非0的点) - int ConvertToSVzNL3DPosition(const std::vector>& unifiedData, - std::vector>& scanLines, - size_t* validPointCount = nullptr); - - // 从文件加载Poly折线数据(带颜色) - // polyLines: 每条折线的点坐标列表,polyLines[i] 是第i条折线的所有点(含RGB颜色) - int LoadPolySegments(const std::string& fileName, - std::vector>& polyLines); + int ConvertToSVzNL3DPosition(const std::vector>& unifiedData, + std::vector>& scanLines, + size_t* validPointCount = nullptr); + + // 转换统一格式数据为std::vector>格式 + int ConvertToSVzNLPositionD(const std::vector>& unifiedData, + std::vector>& scanLines); + + // 从文件加载Poly折线数据(带颜色) + // polyLines: 每条折线的点坐标列表,polyLines[i] 是第i条折线的所有点(含RGB颜色) + int LoadPolySegments(const std::string& fileName, + std::vector>& polyLines); // 获取最后的错误信息 std::string GetLastError() const { return m_lastError; } private: - // 读取XYZ格式的激光数据 - int _ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPosition& s2DData); - - // 读取RGBD格式的激光数据 - 新增RGBD支持 + // 读取XYZ格式的激光数据 + int _ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPosition& s2DData); + int _ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPositionF& s2DData); + + // 读取RGBD格式的激光数据 - 新增RGBD支持 int _ParseLaserScanPoint(const std::string& data, SVzNLPointXYZRGBA& sData, SVzNL2DLRPoint& s2DData); // 获取激光数据类型 diff --git a/CloudUtils/Src/LaserDataLoader.cpp b/CloudUtils/Src/LaserDataLoader.cpp index 99bc738..d66c23d 100644 --- a/CloudUtils/Src/LaserDataLoader.cpp +++ b/CloudUtils/Src/LaserDataLoader.cpp @@ -1,163 +1,163 @@ -#include "LaserDataLoader.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "LaserDataLoader.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "VrLog.h" // 辅助函数:去除字符串末尾的 \r 字符(处理 Windows 格式的 CR LF 换行符) -static inline void TrimCarriageReturn(std::string& str) -{ - if (!str.empty() && str.back() == '\r') { - str.pop_back(); - } -} - +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 -bool ReadBinary(std::ifstream& inputFile, T& value) -{ - inputFile.read(reinterpret_cast(&value), sizeof(T)); - return static_cast(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(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(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(written) < sizeof(stackBuffer)) { - output.append(stackBuffer, static_cast(written)); - va_end(argsCopy); - return; - } - - std::vector dynamicBuffer(static_cast(written) + 1); - vsnprintf(dynamicBuffer.data(), dynamicBuffer.size(), format, argsCopy); - va_end(argsCopy); - output.append(dynamicBuffer.data(), static_cast(written)); -} -} + 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 +bool ReadBinary(std::ifstream& inputFile, T& value) +{ + inputFile.read(reinterpret_cast(&value), sizeof(T)); + return static_cast(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(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(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(written) < sizeof(stackBuffer)) { + output.append(stackBuffer, static_cast(written)); + va_end(argsCopy); + return; + } + + std::vector dynamicBuffer(static_cast(written) + 1); + vsnprintf(dynamicBuffer.data(), dynamicBuffer.size(), format, argsCopy); + va_end(argsCopy); + output.append(dynamicBuffer.data(), static_cast(written)); +} +} LaserDataLoader::LaserDataLoader() { @@ -182,11 +182,11 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName, lineNum = 0; scanSpeed = 0.0f; maxTimeStamp = 0; - clockPerSecond = 0; - - if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) { - return _LoadLaserScanDataFromDatFile(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond); - } + clockPerSecond = 0; + + if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) { + return _LoadLaserScanDataFromDatFile(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond); + } // 判断文件类型 std::ifstream inputFile(fileName); @@ -239,14 +239,14 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName, 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(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 折线头及后续点坐标数据 @@ -285,14 +285,19 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName, SVzNL2DLRPoint* p2DPoints = static_cast(sLaserData.p2DPoint); _ParseLaserScanPoint(line, pRGBAPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]); nLaserPointIdx++; - } else { - SVzNL3DPosition* p3DPoints = static_cast(sLaserData.p3DPoint); - SVzNL2DPosition* p2DPoints = static_cast(sLaserData.p2DPoint); - _ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]); - p3DPoints[nLaserPointIdx].nPointIdx = nLaserPointIdx; - nLaserPointIdx++; - } - } + } else if (eDataType == keResultDataType_Position) { + SVzNL3DPosition* p3DPoints = static_cast(sLaserData.p3DPoint); + SVzNL2DPosition* p2DPoints = static_cast(sLaserData.p2DPoint); + result = _ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]); + if (result != SUCCESS) { + m_lastError = "Invalid XYZUV point line: " + line; + LOG_ERROR("%s\n", m_lastError.c_str()); + return result; + } + p3DPoints[nLaserPointIdx].nPointIdx = nLaserPointIdx; + nLaserPointIdx++; + } + } } // 添加最后一条扫描线数据 @@ -305,125 +310,125 @@ int LaserDataLoader::LoadLaserScanData(const std::string& fileName, return SUCCESS; } -// Read binary .dat laser scan data. -int LaserDataLoader::_LoadLaserScanDataFromDatFile(const std::string& fileName, - std::vector>& 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(sizeof(header))) { - return fail(FILE_ERR_FORMAT, "Invalid dat header: " + fileName); - } - - const int kFeaturePointSize = static_cast(sizeof(Dat3DFeaturePoint)); - auto validateLayout = [&](int lineRecordSize) -> bool { - std::streamoff pos = static_cast(sizeof(header)); - inputFile.clear(); - inputFile.seekg(pos, std::ios::beg); - - for (int i = 0; i < header.scanLineNum; ++i) { - if (pos + lineRecordSize + static_cast(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(sizeof(pointCount)); - - if (pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) { - return false; - } - - const std::streamoff pointBytes = - static_cast(pointCount) * static_cast(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(header.lineV); - maxTimeStamp = static_cast(header.maxTimeStamp); - clockPerSecond = header.clock; - - laserLines.clear(); - laserLines.reserve(static_cast(lineNum)); - - inputFile.clear(); - inputFile.seekg(static_cast(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(datLine.lineIndex); - lineData.llTimeStamp = static_cast(datLine.timeStamp); - +// Read binary .dat laser scan data. +int LaserDataLoader::_LoadLaserScanDataFromDatFile(const std::string& fileName, + std::vector>& 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(sizeof(header))) { + return fail(FILE_ERR_FORMAT, "Invalid dat header: " + fileName); + } + + const int kFeaturePointSize = static_cast(sizeof(Dat3DFeaturePoint)); + auto validateLayout = [&](int lineRecordSize) -> bool { + std::streamoff pos = static_cast(sizeof(header)); + inputFile.clear(); + inputFile.seekg(pos, std::ios::beg); + + for (int i = 0; i < header.scanLineNum; ++i) { + if (pos + lineRecordSize + static_cast(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(sizeof(pointCount)); + + if (pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) { + return false; + } + + const std::streamoff pointBytes = + static_cast(pointCount) * static_cast(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(header.lineV); + maxTimeStamp = static_cast(header.maxTimeStamp); + clockPerSecond = header.clock; + + laserLines.clear(); + laserLines.reserve(static_cast(lineNum)); + + inputFile.clear(); + inputFile.seekg(static_cast(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(datLine.lineIndex); + lineData.llTimeStamp = static_cast(datLine.timeStamp); + std::unique_ptr points3D; std::unique_ptr points2D; @@ -436,44 +441,44 @@ int LaserDataLoader::_LoadLaserScanDataFromDatFile(const std::string& fileName, std::memset(points3D.get(), 0, sizeof(SVzNL3DPosition) * static_cast(pointCount)); std::memset(points2D.get(), 0, sizeof(SVzNL2DPosition) * static_cast(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; - + + 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(featurePoint.stereo.x); points2D[j].ptLeft2D.y = static_cast(featurePoint.stereo.y); points2D[j].ptRight2D.x = static_cast(featurePoint.stereoRight.x); points2D[j].ptRight2D.y = static_cast(featurePoint.stereoRight.y); - } - - lineData.p3DPoint = points3D.get(); - lineData.p2DPoint = points2D.get(); - } - + } + + 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>& laserLines, - int lineNum, - float scanSpeed, + 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>& laserLines, + int lineNum, + float scanSpeed, int maxTimeStamp, int clockPerSecond) { @@ -493,7 +498,7 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName, return ERR_CODE(FILE_ERR_WRITE); } - // 写入文件头 + // 写入文件头 sw << "LineNum:" << lineNum << '\n'; sw << "DataType: 0" << '\n'; sw << "ScanSpeed:" << scanSpeed << '\n'; @@ -506,31 +511,51 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName, EVzResultDataType dataType = linePair.first; const SVzLaserLineData& lineData = linePair.second; - sw << "Line_" << lineData.llFrameIdx << "_" << lineData.llTimeStamp << "_" << lineData.nPointCount << '\n'; + sw << "Line_" << lineData.llFrameIdx << "_" << lineData.llTimeStamp << "_" << lineData.nPointCount << '\n'; // 根据数据类型写入点云数据 - if (dataType == keResultDataType_Position && lineData.p3DPoint) { - // 写入XYZ格式数据 - const SVzNL3DPosition* points = static_cast(lineData.p3DPoint); + if (dataType == keResultDataType_Position && lineData.p3DPoint) { + // 写入XYZ格式数据 + const SVzNL3DPosition* points = static_cast(lineData.p3DPoint); const SVzNL2DPosition* points2D = static_cast(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(points[i].pt3D.x); + float y = static_cast(points[i].pt3D.y); + float z = static_cast(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(buffer.size())); + } + } else if (dataType == keResultDataType_PositionF && lineData.p3DPoint) { + const SVzNL3DPosition* points = static_cast(lineData.p3DPoint); + const SVzNL2DPositionF* points2D = static_cast(lineData.p2DPoint); constexpr int BATCH = 256; - constexpr int PER_POINT = 128; + 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(points[i].pt3D.x); - float y = static_cast(points[i].pt3D.y); - float z = static_cast(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); + AppendFormattedLine(buffer, + "{ %.9f, %.9f, %.9f } - { %.6f, %.6f } - { %.6f, %.6f }\n", + points[i].pt3D.x, points[i].pt3D.y, points[i].pt3D.z, + points2D ? points2D[i].ptLeft2D.x : 0.0f, + points2D ? points2D[i].ptLeft2D.y : 0.0f, + points2D ? points2D[i].ptRight2D.x : 0.0f, + points2D ? points2D[i].ptRight2D.y : 0.0f); } sw.write(buffer.data(), static_cast(buffer.size())); } - } else if (dataType == keResultDataType_PointXYZI && lineData.p3DPoint) { + } else if (dataType == keResultDataType_PointXYZI && lineData.p3DPoint) { // LiDAR 点云:snprintf 批量写入,每 256 点 flush // 坐标 mm 范围可达 ±200000 → "%.6f" 最长 14 字符 → 整行 ≤73 字符 const SVzNLPointXYZI* points = static_cast(lineData.p3DPoint); @@ -552,29 +577,29 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName, } else if (dataType == keResultDataType_PointXYZRGBA && lineData.p3DPoint) { // 写入RGBD格式数据 const SVzNLPointXYZRGBA* points = static_cast(lineData.p3DPoint); - const SVzNL2DLRPoint* points2D = static_cast(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(points[i].x); - float y = static_cast(points[i].y); - float z = static_cast(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(buffer.size())); - } + const SVzNL2DLRPoint* points2D = static_cast(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(points[i].x); + float y = static_cast(points[i].y); + float z = static_cast(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(buffer.size())); + } } } @@ -613,11 +638,11 @@ int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector(BATCH), linePair.size()); - std::string buffer; - buffer.reserve(BATCH * PER_POINT); - for (size_t i = base; i < end; ++i) { - const auto& point = linePair[i]; + 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(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(point.pt3D.x); - float y = static_cast(point.pt3D.y); - float z = static_cast(point.pt3D.z); - AppendFormattedLine(buffer, "{ %.6f, %.6f, %.6f } - { 0, 0 } - { 0, 0 }\n", - x, y, z); - } - sw.write(buffer.data(), static_cast(buffer.size())); - } + float x = static_cast(point.pt3D.x); + float y = static_cast(point.pt3D.y); + float z = static_cast(point.pt3D.z); + AppendFormattedLine(buffer, "{ %.6f, %.6f, %.6f } - { 0, 0 } - { 0, 0 }\n", + x, y, z); + } + sw.write(buffer.data(), static_cast(buffer.size())); + } ++index; if (callback) { @@ -814,61 +839,61 @@ void LaserDataLoader::FreeLaserScanData(std::vector(lineData.p3DPoint); break; case keResultDataType_PositionF: - delete[] static_cast(lineData.p3DPoint); + delete[] static_cast(lineData.p3DPoint); break; - case keResultDataType_PointF: - delete[] static_cast(lineData.p3DPoint); - break; - case keResultDataType_PointXYZ: - delete[] static_cast(lineData.p3DPoint); - break; - case keResultDataType_PointXYZRGBA: - delete[] static_cast(lineData.p3DPoint); - break; - case keResultDataType_PointGray: - delete[] static_cast(lineData.p3DPoint); - break; - case keResultDataType_PointXYZI: - delete[] static_cast(lineData.p3DPoint); - break; - case keResultDataType_PointRGBA_D: - delete[] static_cast(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(lineData.p2DPoint); - break; - case keResultDataType_PositionF: - delete[] static_cast(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(lineData.p2DPoint); - break; - default: - LOG_WARN("Unknown laser data type when freeing p2DPoint: %d\n", dataType); - break; - } - lineData.p2DPoint = nullptr; - } + case keResultDataType_PointF: + delete[] static_cast(lineData.p3DPoint); + break; + case keResultDataType_PointXYZ: + delete[] static_cast(lineData.p3DPoint); + break; + case keResultDataType_PointXYZRGBA: + delete[] static_cast(lineData.p3DPoint); + break; + case keResultDataType_PointGray: + delete[] static_cast(lineData.p3DPoint); + break; + case keResultDataType_PointXYZI: + delete[] static_cast(lineData.p3DPoint); + break; + case keResultDataType_PointRGBA_D: + delete[] static_cast(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(lineData.p2DPoint); + break; + case keResultDataType_PositionF: + delete[] static_cast(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(lineData.p2DPoint); + break; + default: + LOG_WARN("Unknown laser data type when freeing p2DPoint: %d\n", dataType); + break; + } + lineData.p2DPoint = nullptr; + } } laserLines.clear(); } @@ -895,8 +920,8 @@ int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector scanLine; // 为当前扫描线分配空间 @@ -914,10 +939,26 @@ int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector scanLine; + scanLine.reserve(lineData.nPointCount); + + const SVzNL3DPosition* points = static_cast(lineData.p3DPoint); + for (int i = 0; i < lineData.nPointCount; i++) { + scanLine.push_back(points[i]); + + 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) { @@ -928,6 +969,82 @@ int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector>& unifiedData, + std::vector>& scanLines) +{ + LOG_DEBUG("Converting unified data to SVzNLPositionD scan lines format\n"); + + scanLines.clear(); + + for (const auto& linePair : unifiedData) { + if (linePair.first != keResultDataType_Position && + linePair.first != keResultDataType_PositionF) { + continue; + } + + const SVzLaserLineData& lineData = linePair.second; + if (lineData.nPointCount < 0) { + continue; + } + + if (lineData.nPointCount > 0 && (!lineData.p3DPoint || !lineData.p2DPoint)) { + m_lastError = "Missing 3D or 2D point buffer in Position laser data"; + LOG_ERROR("%s, pointCount=%d, p3D=%p, p2D=%p\n", + m_lastError.c_str(), + lineData.nPointCount, + lineData.p3DPoint, + lineData.p2DPoint); + continue; + } + + std::vector scanLine; + scanLine.reserve(static_cast(lineData.nPointCount)); + if (linePair.first == keResultDataType_Position) { + const SVzNL3DPosition* points3D = static_cast(lineData.p3DPoint); + const SVzNL2DPosition* points2D = static_cast(lineData.p2DPoint); + for (int i = 0; i < lineData.nPointCount; ++i) { + SVzNLPositionD point; + std::memset(&point, 0, sizeof(point)); + point.nPointIdx = points3D[i].nPointIdx; + point.pt3D = points3D[i].pt3D; + point.ptLeft2D.x = points2D[i].ptLeft2D.x; + point.ptLeft2D.y = points2D[i].ptLeft2D.y; + point.ptRight2D.x = points2D[i].ptRight2D.x; + point.ptRight2D.y = points2D[i].ptRight2D.y; + scanLine.push_back(point); + } + } else { + const SVzNL3DPosition* points3D = static_cast(lineData.p3DPoint); + const SVzNL2DPositionF* points2D = static_cast(lineData.p2DPoint); + for (int i = 0; i < lineData.nPointCount; ++i) { + SVzNLPositionD point; + std::memset(&point, 0, sizeof(point)); + point.nPointIdx = points3D[i].nPointIdx; + point.pt3D = points3D[i].pt3D; + point.ptLeft2D.x = points2D[i].ptLeft2D.x; + point.ptLeft2D.y = points2D[i].ptLeft2D.y; + point.ptRight2D.x = points2D[i].ptRight2D.x; + point.ptRight2D.y = points2D[i].ptRight2D.y; + scanLine.push_back(point); + } + } + + if (!scanLine.empty()) { + scanLines.push_back(scanLine); + } + } + + if (scanLines.empty()) { + m_lastError = "No Position laser data to convert"; + LOG_ERROR("%s\n", m_lastError.c_str()); + return ERR_CODE(DATA_ERR_INVALID); + } + + LOG_DEBUG("Converted %zu lines to SVzNLPositionD scan lines format\n", scanLines.size()); + return SUCCESS; +} + // 转换统一格式数据为SVzNL3DLaserLine格式 int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector>& unifiedData, std::vector& xyzData) @@ -938,11 +1055,12 @@ int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector 0) { xyzLine.p3DPosition = new SVzNL3DPosition[lineData.nPointCount]; @@ -1041,23 +1159,49 @@ void LaserDataLoader::FreeConvertedData(std::vector& rgbd } -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; +int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPosition& s2DData) +{ + float X = 0.0f, Y = 0.0f, Z = 0.0f; + float leftX = 0.0f, leftY = 0.0f; + float rightX = 0.0f, rightY = 0.0f; + const int parsed = sscanf(data.c_str(), + " { %f , %f , %f } - { %f , %f } - { %f , %f }", + &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + if (parsed != 7) { + return ERR_CODE(FILE_ERR_FORMAT); + } + 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) + return SUCCESS; +} + +int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPositionF& s2DData) +{ + double X = 0.0, Y = 0.0, Z = 0.0; + float leftX = 0.0f, leftY = 0.0f; + float rightX = 0.0f, rightY = 0.0f; + const int parsed = sscanf(data.c_str(), + " { %lf , %lf , %lf } - { %f , %f } - { %f , %f }", + &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + if (parsed != 7) { + return ERR_CODE(FILE_ERR_FORMAT); + } + 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; @@ -1113,24 +1257,24 @@ int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNLPointXYZ } // 获取激光数据类型 -int LaserDataLoader::_GetLaserType(const std::string& fileName, EVzResultDataType& eDataType) -{ - std::ifstream inputFile(fileName); - std::string linedata; +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) { + bool bFind = false; + while (std::getline(inputFile, linedata)) { + TrimCarriageReturn(linedata); + + if (linedata.find("{") == 0) { + // 统计 { 的数量判断格式 + int braceCount = 0; + for (char c : linedata) { if (c == '{') braceCount++; } @@ -1146,16 +1290,16 @@ int LaserDataLoader::_GetLaserType(const std::string& fileName, EVzResultDataTyp 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; - } - } + if (commaCount >= 5) { + // 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry} + eDataType = keResultDataType_PointXYZRGBA; + bFind = true; + } else if (commaCount >= 2) { + // XYZUV文本格式: {x,y,z}-{lx,ly}-{rx,ry} + eDataType = keResultDataType_Position; + bFind = true; + } + } } break; } diff --git a/CloudUtils/Src/PointCloudImageUtils.cpp b/CloudUtils/Src/PointCloudImageUtils.cpp index 3e761f0..c7958d1 100644 --- a/CloudUtils/Src/PointCloudImageUtils.cpp +++ b/CloudUtils/Src/PointCloudImageUtils.cpp @@ -1188,10 +1188,11 @@ int PointCloudImageUtils::GenerateDepthImage( if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue; // 根据数据类型处理点云数据 - if (dataType == keResultDataType_Position) { - SVzNL3DPosition* positions = static_cast(lineData.p3DPoint); - for (int i = 0; i < lineData.nPointCount; i++) { - const SVzNL3DPosition& point = positions[i]; + if (dataType == keResultDataType_Position || + dataType == keResultDataType_PositionF) { + SVzNL3DPosition* positions = static_cast(lineData.p3DPoint); + for (int i = 0; i < lineData.nPointCount; i++) { + const SVzNL3DPosition& point = positions[i]; if (point.pt3D.z < 1e-4) continue; if (point.pt3D.x < xMin) xMin = point.pt3D.x; @@ -1250,10 +1251,11 @@ int PointCloudImageUtils::GenerateDepthImage( if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue; - if (dataType == keResultDataType_Position) { - SVzNL3DPosition* positions = static_cast(lineData.p3DPoint); - for (int i = 0; i < lineData.nPointCount; i++) { - const SVzNL3DPosition& point = positions[i]; + if (dataType == keResultDataType_Position || + dataType == keResultDataType_PositionF) { + SVzNL3DPosition* positions = static_cast(lineData.p3DPoint); + for (int i = 0; i < lineData.nPointCount; i++) { + const SVzNL3DPosition& point = positions[i]; if (point.pt3D.z < 1e-4) continue; // 计算图像坐标 diff --git a/Utils.pro b/Utils.pro index 3478236..143b212 100644 --- a/Utils.pro +++ b/Utils.pro @@ -9,13 +9,17 @@ CloudView3D.file = CloudView3D/CloudView3D.pro CloudView.file = CloudView/CloudView.pro # 添加子项目 -SUBDIRS += \ - VrCommon \ - VrUtils \ - CloudUtils \ - DataUtils +SUBDIRS += \ + VrCommon \ + VrUtils + +!equals(TARGET_APP, "ParkingSpaceGuideView") { + SUBDIRS += \ + CloudUtils \ + DataUtils +} -win32-msvc { +win32-msvc:!equals(TARGET_APP, "ParkingSpaceGuideView") { SUBDIRS += \ CloudView3D \ CloudView