From db52f11dd0f575c9f21fb2e7b3e40ac879132b4b Mon Sep 17 00:00:00 2001 From: YangSn Date: Wed, 28 Oct 2026 10:26:03 +0800 Subject: [PATCH] =?UTF-8?q?update:=E6=95=B0=E6=8D=AE=E5=8A=A0=E8=BD=BD?= =?UTF-8?q?=E4=BF=9D=E5=AD=98=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CloudUtils/Inc/LaserDataLoader.h | 4 + CloudUtils/Src/LaserDataLoader.cpp | 201 +++++++++++++++-------------- 2 files changed, 106 insertions(+), 99 deletions(-) diff --git a/CloudUtils/Inc/LaserDataLoader.h b/CloudUtils/Inc/LaserDataLoader.h index 2c2f108..1551de2 100644 --- a/CloudUtils/Inc/LaserDataLoader.h +++ b/CloudUtils/Inc/LaserDataLoader.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "VZNL_Types.h" #include "VrError.h" #include "PointCloudXYZRange.h" @@ -102,6 +103,9 @@ public: int maxTimeStamp, int clockPerSecond); + static bool WriteLaserScanLine(std::ostream& stream, EVzResultDataType dataType, + const SVzLaserLineData& lineData); + int DebugSaveLaser(std::string fileName, std::vector> xyzData); int DebugSaveLaser(std::string fileName, std::vector> xyzData, ISaveProgressCallback* callback); diff --git a/CloudUtils/Src/LaserDataLoader.cpp b/CloudUtils/Src/LaserDataLoader.cpp index b4c713e..54245b8 100644 --- a/CloudUtils/Src/LaserDataLoader.cpp +++ b/CloudUtils/Src/LaserDataLoader.cpp @@ -1017,104 +1017,12 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName, 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(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 = 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) { - 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) { - // LiDAR 点云：snprintf 批量写入，每 256 点 flush - // 坐标 mm 范围可达 ±200000 → "%.6f" 最长 14 字符 → 整行 ≤73 字符 - const SVzNLPointXYZI* points = static_cast(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(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())); - } - } - } - + for (const auto& linePair : laserLines) { + if (!WriteLaserScanLine(sw, linePair.first, linePair.second)) { + m_lastError = "Failed while writing laser line"; + return ERR_CODE(FILE_ERR_WRITE); + } + } sw.flush(); if (!sw.good()) { m_lastError = "Failed while writing file: " + fileName; @@ -1137,7 +1045,102 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName, } } -int LaserDataLoader::SaveLaserScanDataBinary( +bool LaserDataLoader::WriteLaserScanLine(std::ostream& sw, EVzResultDataType dataType, + const SVzLaserLineData& lineData) +{ + if (lineData.nPointCount < 0 || (lineData.nPointCount > 0 && !lineData.p3DPoint)) return false; + if (dataType != keResultDataType_Position && dataType != keResultDataType_PositionF && + dataType != keResultDataType_PointXYZI && dataType != keResultDataType_PointXYZRGBA) return false; + sw << "Line_" << lineData.llFrameIdx << "_" << lineData.llTimeStamp << "_" << lineData.nPointCount << '\n'; + + if (dataType == keResultDataType_Position && lineData.p3DPoint) { + 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 ? points2D[i].ptLeft2D.x : 0, points2D ? points2D[i].ptLeft2D.y : 0, + points2D ? points2D[i].ptRight2D.x : 0, points2D ? points2D[i].ptRight2D.y : 0); + } + 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 = 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) { + 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) { + const SVzNLPointXYZI* points = static_cast(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) { + 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 ? points2D[i].sLeft.x : 0, points2D ? points2D[i].sLeft.y : 0, + points2D ? points2D[i].sRight.x : 0, points2D ? points2D[i].sRight.y : 0); + } + sw.write(buffer.data(), static_cast(buffer.size())); + } + } + + return sw.good(); +} + +int LaserDataLoader::SaveLaserScanDataBinary( const std::string& fileName, const std::vector>& laserLines, int lineNum,