From 7b84fffd2a410f56b3312369b220031aeda9ffc8 Mon Sep 17 00:00:00 2001 From: YangSn Date: Sat, 10 Oct 2026 19:02:17 +0800 Subject: [PATCH] =?UTF-8?q?style:=E8=B0=83=E6=95=B4=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E7=BB=93=E6=9D=9F=E7=AC=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CloudUtils/CloudUtils.pro | 74 +- CloudUtils/Inc/PointCloudFilter.h | 118 +- CloudUtils/Inc/PointCloudXYZRange.h | 144 +- CloudUtils/Src/LaserDataLoader.cpp | 4018 ++++++++++++------------ CloudUtils/Src/PointCloudFilter.cpp | 766 ++--- CloudView3D/CloudView3D.pro | 60 +- CloudView3D/Inc/CloudView3D.h | 14 +- CloudView3D/Inc/CloudView3DTypes.h | 154 +- CloudViewApp/Inc/CloudViewSettings.h | 54 +- CloudViewApp/Inc/PointCloudGLWidget.h | 12 +- CloudViewApp/Src/CloudViewSettings.cpp | 342 +- CloudViewApp/main.cpp | 204 +- ImageView/ImageView.pro | 34 +- ImageView/Inc/ImageView.h | 148 +- ImageView/Src/ImageView.cpp | 630 ++-- 15 files changed, 3386 insertions(+), 3386 deletions(-) diff --git a/CloudUtils/CloudUtils.pro b/CloudUtils/CloudUtils.pro index 0715e8c..048317e 100644 --- a/CloudUtils/CloudUtils.pro +++ b/CloudUtils/CloudUtils.pro @@ -1,37 +1,37 @@ -# CloudUtils搴撻」鐩厤缃枃浠? - -# 鎸囧畾Qt妯″潡 -QT += core - -# 鎸囧畾椤圭洰妯℃澘涓洪潤鎬佸簱 -TEMPLATE = lib -CONFIG += staticlib - -win32-msvc { - QMAKE_CXXFLAGS += /utf-8 -} - - -# 鎸囧畾C++鏍囧噯 -CONFIG += c++11 - -INCLUDEPATH += $$PWD/../../Device/SDK/VzNLSDK/_Inc -INCLUDEPATH += $$PWD/../../Device/SDK/VzNLSDK/Inc - -INCLUDEPATH += $$PWD/../VrCommon/Inc -INCLUDEPATH += $$PWD/../VrUtils/Inc - -# 鎸囧畾鍖呭惈璺緞 -INCLUDEPATH += ./Inc \ - ./Src - -# 鎸囧畾澶存枃浠? -HEADERS += Inc/LaserDataLoader.h \ - Inc/PointCloudImageUtils.h \ - Inc/PointCloudFilter.h \ - Inc/PointCloudXYZRange.h - -# 鎸囧畾婧愭枃浠? -SOURCES += Src/LaserDataLoader.cpp \ - Src/PointCloudImageUtils.cpp \ - Src/PointCloudFilter.cpp +# CloudUtils搴撻」鐩厤缃枃浠? + +# 鎸囧畾Qt妯″潡 +QT += core + +# 鎸囧畾椤圭洰妯℃澘涓洪潤鎬佸簱 +TEMPLATE = lib +CONFIG += staticlib + +win32-msvc { + QMAKE_CXXFLAGS += /utf-8 +} + + +# 鎸囧畾C++鏍囧噯 +CONFIG += c++11 + +INCLUDEPATH += $$PWD/../../Device/SDK/VzNLSDK/_Inc +INCLUDEPATH += $$PWD/../../Device/SDK/VzNLSDK/Inc + +INCLUDEPATH += $$PWD/../VrCommon/Inc +INCLUDEPATH += $$PWD/../VrUtils/Inc + +# 鎸囧畾鍖呭惈璺緞 +INCLUDEPATH += ./Inc \ + ./Src + +# 鎸囧畾澶存枃浠? +HEADERS += Inc/LaserDataLoader.h \ + Inc/PointCloudImageUtils.h \ + Inc/PointCloudFilter.h \ + Inc/PointCloudXYZRange.h + +# 鎸囧畾婧愭枃浠? +SOURCES += Src/LaserDataLoader.cpp \ + Src/PointCloudImageUtils.cpp \ + Src/PointCloudFilter.cpp diff --git a/CloudUtils/Inc/PointCloudFilter.h b/CloudUtils/Inc/PointCloudFilter.h index 0a3bee3..01a0ad5 100644 --- a/CloudUtils/Inc/PointCloudFilter.h +++ b/CloudUtils/Inc/PointCloudFilter.h @@ -1,59 +1,59 @@ -#ifndef POINTCLOUDFILTER_H -#define POINTCLOUDFILTER_H - -#include -#include - -#include "VZNL_Types.h" - -// Selects the outlier removal algorithm. -enum class PointCloudFilterMethod -{ - None = 0, - ScanLineContinuity, - RadiusOutlier, - StatisticalOutlier -}; - -struct PointCloudFilterParams -{ - PointCloudFilterMethod method = PointCloudFilterMethod::None; - - // ScanLineContinuity parameters. Units follow the point cloud (normally mm). - double continuityThreshold = 20.0; - std::size_t minSegmentLength = 5; - - // RadiusOutlier parameters. minNeighbors excludes the point itself. - double radius = 5.0; - std::size_t minNeighbors = 2; - - // StatisticalOutlier parameters. - std::size_t meanK = 8; - double standardDeviationMultiplier = 1.0; - // Zero estimates a radius from the cloud density; non-zero limits the search. - double searchRadius = 0.0; -}; - -struct PointCloudFilterStats -{ - std::size_t inputPointCount = 0; - std::size_t outputPointCount = 0; - std::size_t removedPointCount = 0; -}; - -class PointCloudFilter -{ -public: - // The input is not modified. Original nPointIdx values are retained. - static bool Filter(const std::vector>& input, - std::vector>& output, - const PointCloudFilterParams& params = PointCloudFilterParams(), - PointCloudFilterStats* stats = nullptr); - - static bool Filter(const std::vector& input, - std::vector& output, - const PointCloudFilterParams& params = PointCloudFilterParams(), - PointCloudFilterStats* stats = nullptr); -}; - -#endif // POINTCLOUDFILTER_H +#ifndef POINTCLOUDFILTER_H +#define POINTCLOUDFILTER_H + +#include +#include + +#include "VZNL_Types.h" + +// Selects the outlier removal algorithm. +enum class PointCloudFilterMethod +{ + None = 0, + ScanLineContinuity, + RadiusOutlier, + StatisticalOutlier +}; + +struct PointCloudFilterParams +{ + PointCloudFilterMethod method = PointCloudFilterMethod::None; + + // ScanLineContinuity parameters. Units follow the point cloud (normally mm). + double continuityThreshold = 20.0; + std::size_t minSegmentLength = 5; + + // RadiusOutlier parameters. minNeighbors excludes the point itself. + double radius = 5.0; + std::size_t minNeighbors = 2; + + // StatisticalOutlier parameters. + std::size_t meanK = 8; + double standardDeviationMultiplier = 1.0; + // Zero estimates a radius from the cloud density; non-zero limits the search. + double searchRadius = 0.0; +}; + +struct PointCloudFilterStats +{ + std::size_t inputPointCount = 0; + std::size_t outputPointCount = 0; + std::size_t removedPointCount = 0; +}; + +class PointCloudFilter +{ +public: + // The input is not modified. Original nPointIdx values are retained. + static bool Filter(const std::vector>& input, + std::vector>& output, + const PointCloudFilterParams& params = PointCloudFilterParams(), + PointCloudFilterStats* stats = nullptr); + + static bool Filter(const std::vector& input, + std::vector& output, + const PointCloudFilterParams& params = PointCloudFilterParams(), + PointCloudFilterStats* stats = nullptr); +}; + +#endif // POINTCLOUDFILTER_H diff --git a/CloudUtils/Inc/PointCloudXYZRange.h b/CloudUtils/Inc/PointCloudXYZRange.h index d73d924..f4923f6 100644 --- a/CloudUtils/Inc/PointCloudXYZRange.h +++ b/CloudUtils/Inc/PointCloudXYZRange.h @@ -1,72 +1,72 @@ -#ifndef POINT_CLOUD_XYZ_RANGE_H -#define POINT_CLOUD_XYZ_RANGE_H - -#include -#include - -// XYZ 范围外点的处理方式。 -// -// ReplaceWithZero 保留点在线内的原始槽位，并仅把三维坐标置为零； -// Remove 则删除范围外点并压缩扫描线。保留槽位对于按扫描线/网格 -// 处理的调用方很重要，因此它也是启用范围过滤时的默认模式。 -enum class PointCloudXYZRangeFilterMode -{ - ReplaceWithZero = 0, - Remove = 1 -}; - -// 兼容不需要区分 XYZ 范围限定名称的调用方。 -using PointCloudRangeFilterMode = PointCloudXYZRangeFilterMode; - -// 点云加载使用的可选轴对齐范围；默认关闭以保持原有读取行为。 -struct PointCloudXYZRange -{ - using FilterMode = PointCloudXYZRangeFilterMode; - - bool enabled = false; - double minX = -std::numeric_limits::infinity(); - double maxX = std::numeric_limits::infinity(); - double minY = -std::numeric_limits::infinity(); - double maxY = std::numeric_limits::infinity(); - double minZ = -std::numeric_limits::infinity(); - double maxZ = std::numeric_limits::infinity(); - // Keep this after the legacy bounds so aggregate initialization remains compatible. - PointCloudXYZRangeFilterMode filterMode = - PointCloudXYZRangeFilterMode::ReplaceWithZero; - - bool isValid() const - { - if (!enabled) - { - return true; - } - return !std::isnan(minX) && !std::isnan(maxX) - && !std::isnan(minY) && !std::isnan(maxY) - && !std::isnan(minZ) && !std::isnan(maxZ) - && minX <= maxX && minY <= maxY && minZ <= maxZ; - } - - bool contains(double x, double y, double z) const - { - if (!enabled) - { - return true; - } - if (!isValid() || !std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) - { - return false; - } - return x >= minX && x <= maxX - && y >= minY && y <= maxY - && z >= minZ && z <= maxZ; - } - - bool isEnabled() const - { - return enabled && isValid(); - } -}; - -using PointCloudRange = PointCloudXYZRange; - -#endif // POINT_CLOUD_XYZ_RANGE_H +#ifndef POINT_CLOUD_XYZ_RANGE_H +#define POINT_CLOUD_XYZ_RANGE_H + +#include +#include + +// XYZ 范围外点的处理方式。 +// +// ReplaceWithZero 保留点在线内的原始槽位，并仅把三维坐标置为零； +// Remove 则删除范围外点并压缩扫描线。保留槽位对于按扫描线/网格 +// 处理的调用方很重要，因此它也是启用范围过滤时的默认模式。 +enum class PointCloudXYZRangeFilterMode +{ + ReplaceWithZero = 0, + Remove = 1 +}; + +// 兼容不需要区分 XYZ 范围限定名称的调用方。 +using PointCloudRangeFilterMode = PointCloudXYZRangeFilterMode; + +// 点云加载使用的可选轴对齐范围；默认关闭以保持原有读取行为。 +struct PointCloudXYZRange +{ + using FilterMode = PointCloudXYZRangeFilterMode; + + bool enabled = false; + double minX = -std::numeric_limits::infinity(); + double maxX = std::numeric_limits::infinity(); + double minY = -std::numeric_limits::infinity(); + double maxY = std::numeric_limits::infinity(); + double minZ = -std::numeric_limits::infinity(); + double maxZ = std::numeric_limits::infinity(); + // Keep this after the legacy bounds so aggregate initialization remains compatible. + PointCloudXYZRangeFilterMode filterMode = + PointCloudXYZRangeFilterMode::ReplaceWithZero; + + bool isValid() const + { + if (!enabled) + { + return true; + } + return !std::isnan(minX) && !std::isnan(maxX) + && !std::isnan(minY) && !std::isnan(maxY) + && !std::isnan(minZ) && !std::isnan(maxZ) + && minX <= maxX && minY <= maxY && minZ <= maxZ; + } + + bool contains(double x, double y, double z) const + { + if (!enabled) + { + return true; + } + if (!isValid() || !std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) + { + return false; + } + return x >= minX && x <= maxX + && y >= minY && y <= maxY + && z >= minZ && z <= maxZ; + } + + bool isEnabled() const + { + return enabled && isValid(); + } +}; + +using PointCloudRange = PointCloudXYZRange; + +#endif // POINT_CLOUD_XYZ_RANGE_H diff --git a/CloudUtils/Src/LaserDataLoader.cpp b/CloudUtils/Src/LaserDataLoader.cpp index 8252118..cdc9c83 100644 --- a/CloudUtils/Src/LaserDataLoader.cpp +++ b/CloudUtils/Src/LaserDataLoader.cpp @@ -1,2009 +1,2009 @@ -#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(); - } -} - -namespace -{ -bool IsShapeBlockHeader(const std::string& line) -{ - static const char* prefixes[] = { - "Segment_", "Segments_", - "Rect_", "Rectangle_", "Surface_", "Face_", "Circle_", "Poly_", "Shape_" - }; - for (const char* prefix : prefixes) { - if (line.find(prefix) == 0) { - return true; - } - } - return false; -} - -int ShapeBlockPointCount(const std::string& line) -{ - const size_t lastUnderscore = line.rfind('_'); - if (lastUnderscore == std::string::npos || lastUnderscore + 1 >= line.size()) { - return 0; - } - const int count = std::atoi(line.c_str() + lastUnderscore + 1); - return count > 0 ? count : 0; -} - -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)); -} - -void FilterPositionLineInPlace(SVzLaserLineData& lineData, int sourcePointCount, const PointCloudXYZRange& range, - EVzResultDataType dataType, std::vector* sourcePointIndices) -{ - if (sourcePointIndices) - { - sourcePointIndices->clear(); - } - if (!lineData.p3DPoint) - { - return; - } - - SVzNL3DPosition* points = static_cast(lineData.p3DPoint); - const int count = std::max(0, std::min(sourcePointCount, lineData.nPointCount)); - if (sourcePointIndices) - { - sourcePointIndices->reserve(static_cast(count)); - } - if (!range.enabled) - { - for (int index = 0; index < count; ++index) - { - if (sourcePointIndices) - { - sourcePointIndices->push_back(points[index].nPointIdx); - } - } - return; - } - - // 默认保留扫描线槽位，仅将范围外点的 XYZ 坐标清零。这样可以 - // 保持原始扫描线长度及点索引，供网格/横向选线等调用方使用。 - if (range.filterMode != PointCloudXYZRangeFilterMode::Remove) - { - for (int index = 0; index < count; ++index) - { - SVzNL3DPosition& point = points[index]; - if (!range.contains(point.pt3D.x, point.pt3D.y, point.pt3D.z)) - { - point.pt3D.x = 0.0; - point.pt3D.y = 0.0; - point.pt3D.z = 0.0; - } - if (sourcePointIndices) - { - sourcePointIndices->push_back(point.nPointIdx); - } - } - return; - } - - int keptCount = 0; - for (int index = 0; index < count; ++index) - { - const SVzNL3DPosition& point = points[index]; - if (!range.contains(point.pt3D.x, point.pt3D.y, point.pt3D.z)) - { - continue; - } - - if (keptCount != index) - { - points[keptCount] = point; - if (lineData.p2DPoint && dataType == keResultDataType_PositionF) - { - SVzNL2DPositionF* points2D = - static_cast(lineData.p2DPoint); - points2D[keptCount] = points2D[index]; - } - else if (lineData.p2DPoint) - { - SVzNL2DPosition* points2D = - static_cast(lineData.p2DPoint); - points2D[keptCount] = points2D[index]; - } - } - if (sourcePointIndices) - { - sourcePointIndices->push_back(point.nPointIdx); - } - ++keptCount; - } - lineData.nPointCount = keptCount; -} - -void FilterRgbaLineInPlace(SVzLaserLineData& lineData, int sourcePointCount, const PointCloudXYZRange& range, - std::vector* sourcePointIndices) -{ - if (sourcePointIndices) - { - sourcePointIndices->clear(); - } - if (!lineData.p3DPoint) - { - return; - } - - SVzNLPointXYZRGBA* points = static_cast(lineData.p3DPoint); - SVzNL2DLRPoint* points2D = static_cast(lineData.p2DPoint); - const int count = std::max(0, std::min(sourcePointCount, lineData.nPointCount)); - if (sourcePointIndices) - { - sourcePointIndices->reserve(static_cast(count)); - } - if (!range.enabled) - { - for (int index = 0; index < count; ++index) - { - if (sourcePointIndices) - { - sourcePointIndices->push_back(index); - } - } - return; - } - - // 默认保留扫描线槽位，仅将范围外点的 XYZ 坐标清零；颜色和 2D - // 坐标保持不变，以免破坏原始点与图像坐标的对应关系。 - if (range.filterMode != PointCloudXYZRangeFilterMode::Remove) - { - for (int index = 0; index < count; ++index) - { - SVzNLPointXYZRGBA& point = points[index]; - if (!range.contains(point.x, point.y, point.z)) - { - point.x = 0.0f; - point.y = 0.0f; - point.z = 0.0f; - } - if (sourcePointIndices) - { - sourcePointIndices->push_back(index); - } - } - return; - } - - int keptCount = 0; - for (int index = 0; index < count; ++index) - { - const SVzNLPointXYZRGBA& point = points[index]; - if (!range.contains(point.x, point.y, point.z)) - { - continue; - } - - if (keptCount != index) - { - points[keptCount] = point; - if (points2D) - { - points2D[keptCount] = points2D[index]; - } - } - if (sourcePointIndices) - { - sourcePointIndices->push_back(index); - } - ++keptCount; - } - lineData.nPointCount = keptCount; -} - -void FilterLaserLineInPlace(SVzLaserLineData& lineData, EVzResultDataType dataType, int sourcePointCount, - const PointCloudXYZRange& range, std::vector* sourcePointIndices) -{ - if (sourcePointIndices) - { - sourcePointIndices->clear(); - } - if (dataType == keResultDataType_PointXYZRGBA) - { - FilterRgbaLineInPlace(lineData, sourcePointCount, range, sourcePointIndices); - } - else if (dataType == keResultDataType_Position || dataType == keResultDataType_PositionF) - { - FilterPositionLineInPlace(lineData, sourcePointCount, range, dataType, sourcePointIndices); - } -} -} - -LaserDataLoader::LaserDataLoader() -{ - m_lastError.clear(); -} - -LaserDataLoader::~LaserDataLoader() -{ -} - -int LaserDataLoader::LoadLaserScanData(const std::string& fileName, - std::vector>& laserLines, - int& lineNum, - float& scanSpeed, - int& maxTimeStamp, - int& clockPerSecond) -{ - const PointCloudXYZRange range; - return LoadLaserScanData(fileName, laserLines, lineNum, scanSpeed, - maxTimeStamp, clockPerSecond, range); -} - -int LaserDataLoader::LoadLaserScanData(const std::string& fileName, - std::vector>& laserLines, - int& lineNum, - float& scanSpeed, - int& maxTimeStamp, - int& clockPerSecond, - const PointCloudXYZRange& range) -{ - LOG_INFO("Loading from file: %s\n", fileName.c_str()); - - // 输出容器拥有上次加载的缓冲区；即使本次范围参数无效，也先释放旧数据。 - FreeLaserScanData(laserLines); - - if (!range.isValid()) { - m_lastError = "Invalid XYZ point-cloud range"; - LOG_ERROR("%s\n", m_lastError.c_str()); - lineNum = 0; - scanSpeed = 0.0f; - maxTimeStamp = 0; - clockPerSecond = 0; - return ERR_CODE(FILE_ERR_FORMAT); - } - - // 清空输出参数 - 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, range); - } - - // 判断文件类型 - 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; - - auto failTextLoad = [&](int code, const std::string& message) -> int { - m_lastError = message; - LOG_ERROR("%s\n", message.c_str()); - if (!bFindLineNum) { - laserLines.push_back(std::make_pair(eDataType, sLaserData)); - bFindLineNum = true; - } - FreeLaserScanData(laserLines); - lineNum = 0; - return ERR_CODE(code); - }; - - auto commitCurrentLine = [&]() -> int { - if (bFindLineNum) { - return SUCCESS; - } - - // 先校验原始记录数，再压缩范围外的点，避免把损坏文件当成成功。 - if (nLaserPointIdx != sLaserData.nPointCount) { - std::ostringstream message; - message << "Incomplete laser line: expected " << sLaserData.nPointCount - << " points, got " << nLaserPointIdx; - return failTextLoad(FILE_ERR_FORMAT, message.str()); - } - FilterLaserLineInPlace(sLaserData, eDataType, nLaserPointIdx, range, nullptr); - laserLines.push_back(std::make_pair(eDataType, sLaserData)); - bFindLineNum = true; - return SUCCESS; - }; - - while (std::getline(inputFile, line)) { - // 去除行末的 \r 字符（处理 Windows 格式的 CR LF 换行符） - TrimCarriageReturn(line); - - if (line.find("LineNum:") == 0) { - if (1 != sscanf(line.c_str(), "LineNum:%d", &lineNum) || lineNum <= 0) { - return failTextLoad(FILE_ERR_FORMAT, "Invalid LineNum header: " + line); - } - } else if (line.find("DataType:") == 0) { - - } else if (line.find("Line_") == 0) { - - if(false == bFindLineNum) { - result = commitCurrentLine(); - if (SUCCESS != result) return result; - } - - int lineIndex = 0; - unsigned int timeStamp = 0; - int ptNum = 0; - if (3 != sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum) || - ptNum < 0 || ptNum > VZ_LASER_LINE_PT_MAX_NUM) { - return failTextLoad(FILE_ERR_FORMAT, "Invalid laser line header: " + line); - } - - 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 (IsShapeBlockHeader(line)) { - // 跳过图形块头及后续点坐标数据。 - const int shapePointCount = ShapeBlockPointCount(line); - for (int skip = 0; skip < shapePointCount; ) { - if (!std::getline(inputFile, line)) { - return failTextLoad(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT, - "Incomplete shape block in laser file: " + fileName); - } - 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 failTextLoad(DATA_ERR_INVALID, - "Point data does not match laser line header: " + line); - } - - // 防止数组越界：跳过超出声明点数的数据行 - if (nLaserPointIdx >= sLaserData.nPointCount) { - LOG_WARN("nLaserPointIdx(%d) >= nPointCount(%d), skip\n", nLaserPointIdx, sLaserData.nPointCount); - continue; - } - - if (eDataType == keResultDataType_PointXYZRGBA) { - SVzNLPointXYZRGBA* pRGBAPoints = static_cast(sLaserData.p3DPoint); - SVzNL2DLRPoint* p2DPoints = static_cast(sLaserData.p2DPoint); - result = _ParseLaserScanPoint(line, pRGBAPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]); - if (SUCCESS != result) { - return failTextLoad(FILE_ERR_FORMAT, "Invalid RGBD point: " + line); - } - nLaserPointIdx++; - } else { - SVzNL3DPosition* p3DPoints = static_cast(sLaserData.p3DPoint); - SVzNL2DPosition* p2DPoints = static_cast(sLaserData.p2DPoint); - result = _ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]); - if (SUCCESS != result) { - return failTextLoad(FILE_ERR_FORMAT, "Invalid XYZ point: " + line); - } - p3DPoints[nLaserPointIdx].nPointIdx = nLaserPointIdx; - nLaserPointIdx++; - } - } - } - - // 添加最后一条扫描线数据 - if (!bFindLineNum) { - result = commitCurrentLine(); - if (result != SUCCESS) { - return result; - } - } - - inputFile.close(); - LOG_INFO("Successfully loaded %d laser scan lines from file: %s\n", lineNum, fileName.c_str()); - return SUCCESS; -} - -int LaserDataLoader::LoadLaserScanDataByCallback( - const std::string& fileName, - const LaserLineLoadCallback& lineCallback, - const LaserLoadFinishedCallback& finishedCallback) -{ - const PointCloudXYZRange range; - return LoadLaserScanDataByCallback(fileName, lineCallback, finishedCallback, range); -} - -int LaserDataLoader::LoadLaserScanDataByCallback( - const std::string& fileName, - const LaserLineLoadCallback& lineCallback, - const LaserLoadFinishedCallback& finishedCallback, - const PointCloudXYZRange& range) -{ - const LaserLineLoadIndexedCallback indexedCallback = - [lineCallback](EVzResultDataType dataType, SVzLaserLineData& lineData, - const LaserLineLoadInfo&) - { - if (lineCallback) - { - lineCallback(dataType, lineData); - } - }; - return LoadLaserScanDataByIndexedCallback(fileName, indexedCallback, - finishedCallback, range); -} - -int LaserDataLoader::LoadLaserScanDataByIndexedCallback( - const std::string& fileName, - const LaserLineLoadIndexedCallback& lineCallback, - const LaserLoadFinishedCallback& finishedCallback, - const PointCloudXYZRange& range) -{ - LOG_INFO("Loading from file by callback: %s\n", fileName.c_str()); - m_lastError.clear(); - - if (!range.isValid()) { - m_lastError = "Invalid XYZ point-cloud range"; - LOG_ERROR("%s\n", m_lastError.c_str()); - if (finishedCallback) { - finishedCallback(ERR_CODE(FILE_ERR_FORMAT), 0, 0.0f, 0, 0); - } - return ERR_CODE(FILE_ERR_FORMAT); - } - - int lineNum = 0; - float scanSpeed = 0.0f; - int maxTimeStamp = 0; - int clockPerSecond = 0; - - auto finish = [&](int result) -> int { - if (finishedCallback) { - finishedCallback(result, lineNum, scanSpeed, maxTimeStamp, clockPerSecond); - } - return result; - }; - - if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) { - return _LoadLaserScanDataFromDatFileByIndexedCallback( - fileName, lineCallback, finishedCallback, range); - } - - 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 finish(ERR_CODE(FILE_ERR_NOEXIST)); - } - - EVzResultDataType dataType = keResultDataType_Invalid; - int result = _GetLaserType(fileName, dataType); - if (result != SUCCESS) { - m_lastError = "Cannot determine laser data type: " + fileName; - LOG_ERROR("%s\n", m_lastError.c_str()); - return finish(result); - } - - SVzLaserLineData lineData; - std::memset(&lineData, 0, sizeof(lineData)); - std::unique_ptr positionPoints; - std::unique_ptr position2DPoints; - std::unique_ptr rgbaPoints; - std::unique_ptr rgba2DPoints; - bool hasLine = false; - int pointIndex = 0; - - auto emitLine = [&]() -> int { - if (!hasLine) { - return SUCCESS; - } - if (pointIndex != lineData.nPointCount) { - std::ostringstream message; - message << "Laser line " << lineData.llFrameIdx << " declares " - << lineData.nPointCount << " points but contains " << pointIndex; - m_lastError = message.str(); - LOG_ERROR("%s\n", m_lastError.c_str()); - return ERR_CODE(FILE_ERR_FORMAT); - } - LaserLineLoadInfo loadInfo; - loadInfo.sourcePointCount = lineData.nPointCount; - FilterLaserLineInPlace(lineData, dataType, loadInfo.sourcePointCount, range, - &loadInfo.sourcePointIndices); - if (lineCallback) { - lineCallback(dataType, lineData, loadInfo); - } - positionPoints.reset(); - position2DPoints.reset(); - rgbaPoints.reset(); - rgba2DPoints.reset(); - std::memset(&lineData, 0, sizeof(lineData)); - hasLine = false; - pointIndex = 0; - return SUCCESS; - }; - - std::string line; - while (std::getline(inputFile, line)) { - TrimCarriageReturn(line); - - if (line.find("LineNum:") == 0) { - sscanf(line.c_str(), "LineNum:%d", &lineNum); - } else if (line.find("ScanSpeed:") == 0) { - sscanf(line.c_str(), "ScanSpeed:%f", &scanSpeed); - } else if (line.find("MaxTimeStamp:") == 0) { - sscanf(line.c_str(), "MaxTimeStamp:%d_%d", &maxTimeStamp, &clockPerSecond); - } else if (line.find("Line_") == 0) { - result = emitLine(); - if (result != SUCCESS) { - return finish(result); - } - - int lineIndex = 0; - unsigned int timeStamp = 0; - int pointCount = 0; - if (sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &pointCount) != 3 || - pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) { - m_lastError = "Invalid laser line header: " + line; - LOG_ERROR("%s\n", m_lastError.c_str()); - return finish(ERR_CODE(FILE_ERR_FORMAT)); - } - - lineData.llFrameIdx = lineIndex; - lineData.llTimeStamp = timeStamp; - lineData.nPointCount = pointCount; - - if (pointCount > 0 && dataType == keResultDataType_PointXYZRGBA) { - rgbaPoints.reset(new (std::nothrow) SVzNLPointXYZRGBA[pointCount]); - rgba2DPoints.reset(new (std::nothrow) SVzNL2DLRPoint[pointCount]); - if (!rgbaPoints || !rgba2DPoints) { - m_lastError = "Memory allocation failed while reading: " + fileName; - LOG_ERROR("%s\n", m_lastError.c_str()); - return finish(ERR_CODE(DATA_ERR_MEM)); - } - std::memset(rgbaPoints.get(), 0, - sizeof(SVzNLPointXYZRGBA) * static_cast(pointCount)); - std::memset(rgba2DPoints.get(), 0, - sizeof(SVzNL2DLRPoint) * static_cast(pointCount)); - lineData.p3DPoint = rgbaPoints.get(); - lineData.p2DPoint = rgba2DPoints.get(); - } else if (pointCount > 0 && dataType == keResultDataType_Position) { - positionPoints.reset(new (std::nothrow) SVzNL3DPosition[pointCount]); - position2DPoints.reset(new (std::nothrow) SVzNL2DPosition[pointCount]); - if (!positionPoints || !position2DPoints) { - m_lastError = "Memory allocation failed while reading: " + fileName; - LOG_ERROR("%s\n", m_lastError.c_str()); - return finish(ERR_CODE(DATA_ERR_MEM)); - } - std::memset(positionPoints.get(), 0, - sizeof(SVzNL3DPosition) * static_cast(pointCount)); - std::memset(position2DPoints.get(), 0, - sizeof(SVzNL2DPosition) * static_cast(pointCount)); - lineData.p3DPoint = positionPoints.get(); - lineData.p2DPoint = position2DPoints.get(); - } - hasLine = true; - if (pointCount == 0) { - result = emitLine(); - if (result != SUCCESS) { - return finish(result); - } - } - } else if (IsShapeBlockHeader(line)) { - const int shapePointCount = ShapeBlockPointCount(line); - for (int skipped = 0; skipped < shapePointCount;) { - if (!std::getline(inputFile, line)) { - break; - } - TrimCarriageReturn(line); - if (!line.empty()) { - ++skipped; - } - } - } else if (line.find("{") == 0) { - if (!hasLine || pointIndex >= lineData.nPointCount || - !lineData.p3DPoint || !lineData.p2DPoint) { - m_lastError = "Point data does not match laser line header: " + line; - LOG_ERROR("%s\n", m_lastError.c_str()); - return finish(ERR_CODE(FILE_ERR_FORMAT)); - } - - if (dataType == keResultDataType_PointXYZRGBA) { - result = _ParseLaserScanPoint(line, rgbaPoints[pointIndex], rgba2DPoints[pointIndex]); - } else { - result = _ParseLaserScanPoint(line, positionPoints[pointIndex], position2DPoints[pointIndex]); - positionPoints[pointIndex].nPointIdx = pointIndex; - } - if (result != SUCCESS) { - m_lastError = "Invalid point line: " + line; - LOG_ERROR("%s\n", m_lastError.c_str()); - return finish(result); - } - ++pointIndex; - if (pointIndex == lineData.nPointCount) { - result = emitLine(); - if (result != SUCCESS) { - return finish(result); - } - } - } - } - - result = emitLine(); - if (result != SUCCESS) { - return finish(result); - } - LOG_INFO("Successfully loaded %d laser scan lines by callback from file: %s\n", - lineNum, fileName.c_str()); - return finish(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, - const PointCloudXYZRange& range) -{ - 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; - - 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(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; - - 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(); - } - - FilterLaserLineInPlace(lineData, keResultDataType_Position, pointCount, range, nullptr); - - 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, - 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(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())); - } - } - } - - sw.flush(); - if (!sw.good()) { - m_lastError = "Failed while writing file: " + fileName; - LOG_ERROR("Failed while writing file: %s\n", fileName.c_str()); - return ERR_CODE(FILE_ERR_WRITE); - } - 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 > xyzData) -{ - return DebugSaveLaser(std::move(fileName), std::move(xyzData), nullptr); -} - -int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector> 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(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())); - } - - ++index; - if (callback) { - callback->onSaveProgress(static_cast(index) / static_cast(totalLines)); - } - } - - sw.flush(); - if (!sw.good()) { - m_lastError = "Failed while writing file: " + fileName; - LOG_ERROR("Failed while writing file: %s\n", fileName.c_str()); - return ERR_CODE(FILE_ERR_WRITE); - } - 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::LoadShapeBlocks(const std::string& fileName, - std::vector>& shapeBlocks) -{ - LOG_INFO("Loading shape blocks from file: %s\n", fileName.c_str()); - - shapeBlocks.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; - } - - // 查找图形块头。 - if (!IsShapeBlockHeader(line)) { - continue; - } - - // 解析点数量（最后一个下划线后的数字） - const int num = ShapeBlockPointCount(line); - if (num < 2) { - LOG_WARN("[LaserDataLoader] Invalid shape header: %s\n", line.c_str()); - continue; - } - - // 读取 num 个点坐标 - std::vector points; - bool incompleteBlock = false; - for (int i = 0; i < num; ) { - if (!std::getline(inputFile, line)) { - incompleteBlock = true; - 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(A) << 24; - nRGB |= static_cast(B) << 16; - nRGB |= static_cast(G) << 8; - nRGB |= static_cast(R); - pt.nRGB = nRGB; - } - parsed = true; - } - - // Compact coloured shape format: {x,y,z} - {R,G,B,A}. - if (!parsed) { - int cr = 255, cg = 255, cb = 255, ca = 255; - if (sscanf(line.c_str(), - " { %f , %f , %f } - { %d , %d , %d , %d }", - &fx, &fy, &fz, &cr, &cg, &cb, &ca) == 7) { - pt.x = fx; - pt.y = fy; - pt.z = fz; - pt.nRGB = (static_cast(ca) << 24) | - (static_cast(cb) << 16) | - (static_cast(cg) << 8) | - static_cast(cr); - 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 (incompleteBlock || points.size() != static_cast(num)) { - m_lastError = "Incomplete or invalid shape block in file: " + fileName; - LOG_ERROR("%s\n", m_lastError.c_str()); - shapeBlocks.clear(); - return ERR_CODE(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT); - } - shapeBlocks.push_back(std::move(points)); - } - - if (inputFile.bad()) { - m_lastError = "Failed while reading shape file: " + fileName; - LOG_ERROR("%s\n", m_lastError.c_str()); - shapeBlocks.clear(); - return ERR_CODE(FILE_ERR_READ); - } - - inputFile.close(); - LOG_INFO("Loaded %zu shape blocks from file: %s\n", shapeBlocks.size(), fileName.c_str()); - return SUCCESS; -} - -int LaserDataLoader::LoadPolySegments( - const std::string& fileName, - std::vector>& polyLines) -{ - return LoadShapeBlocks(fileName, polyLines); -} - -void LaserDataLoader::FreeLaserScanData(std::vector>& 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(lineData.p3DPoint); - break; - case keResultDataType_PositionF: - 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; - } - } - laserLines.clear(); - } - - LOG_DEBUG("Unified laser scan data freed successfully\n"); -} - -// 转换统一格式数据为std::vector>格式 -int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector>& unifiedData, - std::vector>& scanLines, - size_t* validPointCount) -{ - 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; - - if (lineData.nPointCount < 0) { - continue; - } - - // 只处理Position类型的数据；过滤后为空的扫描线也要保留位置。 - if ((dataType == keResultDataType_Position || - dataType == keResultDataType_PositionF) && - (lineData.p3DPoint || lineData.nPointCount == 0)) { - std::vector scanLine; - scanLine.reserve(lineData.nPointCount); - - const SVzNL3DPosition* points = static_cast(lineData.p3DPoint); - for (int i = 0; i < lineData.nPointCount; ++i) { - if (points == nullptr) { - break; - } - 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; -} - -int LaserDataLoader::ConvertToSVzNLPositionD( - 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) -{ - xyzData.clear(); - - for (const auto& linePair : unifiedData) { - EVzResultDataType dataType = linePair.first; - const SVzLaserLineData& lineData = linePair.second; - - // PositionF 的 3D buffer 也是 SVzNL3DPosition；仅 2D UV 为浮点。 - if ((dataType == keResultDataType_Position || - dataType == keResultDataType_PositionF) && - (lineData.p3DPoint || lineData.nPointCount == 0)) { - 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>& unifiedData, - std::vector& 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 || lineData.nPointCount == 0)) { - 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& 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& 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 = 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::_LoadLaserScanDataFromDatFileByIndexedCallback( - const std::string& fileName, - const LaserLineLoadIndexedCallback& lineCallback, - const LaserLoadFinishedCallback& finishedCallback, - const PointCloudXYZRange& range) -{ - int lineNum = 0; - float scanSpeed = 0.0f; - int maxTimeStamp = 0; - int clockPerSecond = 0; - - auto finish = [&](int result) -> int { - if (finishedCallback) { - finishedCallback(result, lineNum, scanSpeed, maxTimeStamp, clockPerSecond); - } - return result; - }; - - 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 finish(ERR_CODE(FILE_ERR_NOEXIST)); - } - - auto fail = [&](int code, const std::string& message) -> int { - m_lastError = message; - LOG_ERROR("%s\n", message.c_str()); - return finish(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 featurePointSize = 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) || - pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) { - return false; - } - pos += static_cast(sizeof(pointCount)); - const std::streamoff pointBytes = - static_cast(pointCount) * featurePointSize; - 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; - 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); - } - - std::unique_ptr points3D; - std::unique_ptr 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(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; - 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); - } - - 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); - lineData.p3DPoint = points3D.get(); - lineData.p2DPoint = points2D.get(); - LaserLineLoadInfo loadInfo; - loadInfo.sourcePointCount = pointCount; - FilterLaserLineInPlace(lineData, keResultDataType_Position, pointCount, range, - &loadInfo.sourcePointIndices); - if (lineCallback) { - lineCallback(keResultDataType_Position, lineData, loadInfo); - } - } - - LOG_INFO("Successfully loaded %d dat laser scan lines by callback from file: %s\n", - lineNum, fileName.c_str()); - return finish(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 = 0.0f, Y = 0.0f, Z = 0.0f; - float leftX = 0.0f, leftY = 0.0f; - float rightX = 0.0f, rightY = 0.0f; - - // 尝试新格式: {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(A) << 24; - nRGB |= static_cast(B) << 16; - nRGB |= static_cast(G) << 8; - nRGB |= static_cast(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 = 0.0f, g = 0.0f, b = 0.0f; - if (10 != sscanf(data.c_str(), - " { %f , %f , %f , %f , %f , %f } - { %f , %f } - { %f , %f }", - &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY)) { - return ERR_CODE(FILE_ERR_FORMAT); - } - 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) { - // XYZUV文本格式: {x,y,z}-{lx,ly}-{rx,ry} - eDataType = keResultDataType_Position; - bFind = true; - } - } - } - break; - } - } - - inputFile.close(); - return bFind ? SUCCESS : ERR_CODE(FILE_ERR_FORMAT); -} +#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(); + } +} + +namespace +{ +bool IsShapeBlockHeader(const std::string& line) +{ + static const char* prefixes[] = { + "Segment_", "Segments_", + "Rect_", "Rectangle_", "Surface_", "Face_", "Circle_", "Poly_", "Shape_" + }; + for (const char* prefix : prefixes) { + if (line.find(prefix) == 0) { + return true; + } + } + return false; +} + +int ShapeBlockPointCount(const std::string& line) +{ + const size_t lastUnderscore = line.rfind('_'); + if (lastUnderscore == std::string::npos || lastUnderscore + 1 >= line.size()) { + return 0; + } + const int count = std::atoi(line.c_str() + lastUnderscore + 1); + return count > 0 ? count : 0; +} + +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)); +} + +void FilterPositionLineInPlace(SVzLaserLineData& lineData, int sourcePointCount, const PointCloudXYZRange& range, + EVzResultDataType dataType, std::vector* sourcePointIndices) +{ + if (sourcePointIndices) + { + sourcePointIndices->clear(); + } + if (!lineData.p3DPoint) + { + return; + } + + SVzNL3DPosition* points = static_cast(lineData.p3DPoint); + const int count = std::max(0, std::min(sourcePointCount, lineData.nPointCount)); + if (sourcePointIndices) + { + sourcePointIndices->reserve(static_cast(count)); + } + if (!range.enabled) + { + for (int index = 0; index < count; ++index) + { + if (sourcePointIndices) + { + sourcePointIndices->push_back(points[index].nPointIdx); + } + } + return; + } + + // 默认保留扫描线槽位，仅将范围外点的 XYZ 坐标清零。这样可以 + // 保持原始扫描线长度及点索引，供网格/横向选线等调用方使用。 + if (range.filterMode != PointCloudXYZRangeFilterMode::Remove) + { + for (int index = 0; index < count; ++index) + { + SVzNL3DPosition& point = points[index]; + if (!range.contains(point.pt3D.x, point.pt3D.y, point.pt3D.z)) + { + point.pt3D.x = 0.0; + point.pt3D.y = 0.0; + point.pt3D.z = 0.0; + } + if (sourcePointIndices) + { + sourcePointIndices->push_back(point.nPointIdx); + } + } + return; + } + + int keptCount = 0; + for (int index = 0; index < count; ++index) + { + const SVzNL3DPosition& point = points[index]; + if (!range.contains(point.pt3D.x, point.pt3D.y, point.pt3D.z)) + { + continue; + } + + if (keptCount != index) + { + points[keptCount] = point; + if (lineData.p2DPoint && dataType == keResultDataType_PositionF) + { + SVzNL2DPositionF* points2D = + static_cast(lineData.p2DPoint); + points2D[keptCount] = points2D[index]; + } + else if (lineData.p2DPoint) + { + SVzNL2DPosition* points2D = + static_cast(lineData.p2DPoint); + points2D[keptCount] = points2D[index]; + } + } + if (sourcePointIndices) + { + sourcePointIndices->push_back(point.nPointIdx); + } + ++keptCount; + } + lineData.nPointCount = keptCount; +} + +void FilterRgbaLineInPlace(SVzLaserLineData& lineData, int sourcePointCount, const PointCloudXYZRange& range, + std::vector* sourcePointIndices) +{ + if (sourcePointIndices) + { + sourcePointIndices->clear(); + } + if (!lineData.p3DPoint) + { + return; + } + + SVzNLPointXYZRGBA* points = static_cast(lineData.p3DPoint); + SVzNL2DLRPoint* points2D = static_cast(lineData.p2DPoint); + const int count = std::max(0, std::min(sourcePointCount, lineData.nPointCount)); + if (sourcePointIndices) + { + sourcePointIndices->reserve(static_cast(count)); + } + if (!range.enabled) + { + for (int index = 0; index < count; ++index) + { + if (sourcePointIndices) + { + sourcePointIndices->push_back(index); + } + } + return; + } + + // 默认保留扫描线槽位，仅将范围外点的 XYZ 坐标清零；颜色和 2D + // 坐标保持不变，以免破坏原始点与图像坐标的对应关系。 + if (range.filterMode != PointCloudXYZRangeFilterMode::Remove) + { + for (int index = 0; index < count; ++index) + { + SVzNLPointXYZRGBA& point = points[index]; + if (!range.contains(point.x, point.y, point.z)) + { + point.x = 0.0f; + point.y = 0.0f; + point.z = 0.0f; + } + if (sourcePointIndices) + { + sourcePointIndices->push_back(index); + } + } + return; + } + + int keptCount = 0; + for (int index = 0; index < count; ++index) + { + const SVzNLPointXYZRGBA& point = points[index]; + if (!range.contains(point.x, point.y, point.z)) + { + continue; + } + + if (keptCount != index) + { + points[keptCount] = point; + if (points2D) + { + points2D[keptCount] = points2D[index]; + } + } + if (sourcePointIndices) + { + sourcePointIndices->push_back(index); + } + ++keptCount; + } + lineData.nPointCount = keptCount; +} + +void FilterLaserLineInPlace(SVzLaserLineData& lineData, EVzResultDataType dataType, int sourcePointCount, + const PointCloudXYZRange& range, std::vector* sourcePointIndices) +{ + if (sourcePointIndices) + { + sourcePointIndices->clear(); + } + if (dataType == keResultDataType_PointXYZRGBA) + { + FilterRgbaLineInPlace(lineData, sourcePointCount, range, sourcePointIndices); + } + else if (dataType == keResultDataType_Position || dataType == keResultDataType_PositionF) + { + FilterPositionLineInPlace(lineData, sourcePointCount, range, dataType, sourcePointIndices); + } +} +} + +LaserDataLoader::LaserDataLoader() +{ + m_lastError.clear(); +} + +LaserDataLoader::~LaserDataLoader() +{ +} + +int LaserDataLoader::LoadLaserScanData(const std::string& fileName, + std::vector>& laserLines, + int& lineNum, + float& scanSpeed, + int& maxTimeStamp, + int& clockPerSecond) +{ + const PointCloudXYZRange range; + return LoadLaserScanData(fileName, laserLines, lineNum, scanSpeed, + maxTimeStamp, clockPerSecond, range); +} + +int LaserDataLoader::LoadLaserScanData(const std::string& fileName, + std::vector>& laserLines, + int& lineNum, + float& scanSpeed, + int& maxTimeStamp, + int& clockPerSecond, + const PointCloudXYZRange& range) +{ + LOG_INFO("Loading from file: %s\n", fileName.c_str()); + + // 输出容器拥有上次加载的缓冲区；即使本次范围参数无效，也先释放旧数据。 + FreeLaserScanData(laserLines); + + if (!range.isValid()) { + m_lastError = "Invalid XYZ point-cloud range"; + LOG_ERROR("%s\n", m_lastError.c_str()); + lineNum = 0; + scanSpeed = 0.0f; + maxTimeStamp = 0; + clockPerSecond = 0; + return ERR_CODE(FILE_ERR_FORMAT); + } + + // 清空输出参数 + 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, range); + } + + // 判断文件类型 + 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; + + auto failTextLoad = [&](int code, const std::string& message) -> int { + m_lastError = message; + LOG_ERROR("%s\n", message.c_str()); + if (!bFindLineNum) { + laserLines.push_back(std::make_pair(eDataType, sLaserData)); + bFindLineNum = true; + } + FreeLaserScanData(laserLines); + lineNum = 0; + return ERR_CODE(code); + }; + + auto commitCurrentLine = [&]() -> int { + if (bFindLineNum) { + return SUCCESS; + } + + // 先校验原始记录数，再压缩范围外的点，避免把损坏文件当成成功。 + if (nLaserPointIdx != sLaserData.nPointCount) { + std::ostringstream message; + message << "Incomplete laser line: expected " << sLaserData.nPointCount + << " points, got " << nLaserPointIdx; + return failTextLoad(FILE_ERR_FORMAT, message.str()); + } + FilterLaserLineInPlace(sLaserData, eDataType, nLaserPointIdx, range, nullptr); + laserLines.push_back(std::make_pair(eDataType, sLaserData)); + bFindLineNum = true; + return SUCCESS; + }; + + while (std::getline(inputFile, line)) { + // 去除行末的 \r 字符（处理 Windows 格式的 CR LF 换行符） + TrimCarriageReturn(line); + + if (line.find("LineNum:") == 0) { + if (1 != sscanf(line.c_str(), "LineNum:%d", &lineNum) || lineNum <= 0) { + return failTextLoad(FILE_ERR_FORMAT, "Invalid LineNum header: " + line); + } + } else if (line.find("DataType:") == 0) { + + } else if (line.find("Line_") == 0) { + + if(false == bFindLineNum) { + result = commitCurrentLine(); + if (SUCCESS != result) return result; + } + + int lineIndex = 0; + unsigned int timeStamp = 0; + int ptNum = 0; + if (3 != sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum) || + ptNum < 0 || ptNum > VZ_LASER_LINE_PT_MAX_NUM) { + return failTextLoad(FILE_ERR_FORMAT, "Invalid laser line header: " + line); + } + + 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 (IsShapeBlockHeader(line)) { + // 跳过图形块头及后续点坐标数据。 + const int shapePointCount = ShapeBlockPointCount(line); + for (int skip = 0; skip < shapePointCount; ) { + if (!std::getline(inputFile, line)) { + return failTextLoad(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT, + "Incomplete shape block in laser file: " + fileName); + } + 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 failTextLoad(DATA_ERR_INVALID, + "Point data does not match laser line header: " + line); + } + + // 防止数组越界：跳过超出声明点数的数据行 + if (nLaserPointIdx >= sLaserData.nPointCount) { + LOG_WARN("nLaserPointIdx(%d) >= nPointCount(%d), skip\n", nLaserPointIdx, sLaserData.nPointCount); + continue; + } + + if (eDataType == keResultDataType_PointXYZRGBA) { + SVzNLPointXYZRGBA* pRGBAPoints = static_cast(sLaserData.p3DPoint); + SVzNL2DLRPoint* p2DPoints = static_cast(sLaserData.p2DPoint); + result = _ParseLaserScanPoint(line, pRGBAPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]); + if (SUCCESS != result) { + return failTextLoad(FILE_ERR_FORMAT, "Invalid RGBD point: " + line); + } + nLaserPointIdx++; + } else { + SVzNL3DPosition* p3DPoints = static_cast(sLaserData.p3DPoint); + SVzNL2DPosition* p2DPoints = static_cast(sLaserData.p2DPoint); + result = _ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]); + if (SUCCESS != result) { + return failTextLoad(FILE_ERR_FORMAT, "Invalid XYZ point: " + line); + } + p3DPoints[nLaserPointIdx].nPointIdx = nLaserPointIdx; + nLaserPointIdx++; + } + } + } + + // 添加最后一条扫描线数据 + if (!bFindLineNum) { + result = commitCurrentLine(); + if (result != SUCCESS) { + return result; + } + } + + inputFile.close(); + LOG_INFO("Successfully loaded %d laser scan lines from file: %s\n", lineNum, fileName.c_str()); + return SUCCESS; +} + +int LaserDataLoader::LoadLaserScanDataByCallback( + const std::string& fileName, + const LaserLineLoadCallback& lineCallback, + const LaserLoadFinishedCallback& finishedCallback) +{ + const PointCloudXYZRange range; + return LoadLaserScanDataByCallback(fileName, lineCallback, finishedCallback, range); +} + +int LaserDataLoader::LoadLaserScanDataByCallback( + const std::string& fileName, + const LaserLineLoadCallback& lineCallback, + const LaserLoadFinishedCallback& finishedCallback, + const PointCloudXYZRange& range) +{ + const LaserLineLoadIndexedCallback indexedCallback = + [lineCallback](EVzResultDataType dataType, SVzLaserLineData& lineData, + const LaserLineLoadInfo&) + { + if (lineCallback) + { + lineCallback(dataType, lineData); + } + }; + return LoadLaserScanDataByIndexedCallback(fileName, indexedCallback, + finishedCallback, range); +} + +int LaserDataLoader::LoadLaserScanDataByIndexedCallback( + const std::string& fileName, + const LaserLineLoadIndexedCallback& lineCallback, + const LaserLoadFinishedCallback& finishedCallback, + const PointCloudXYZRange& range) +{ + LOG_INFO("Loading from file by callback: %s\n", fileName.c_str()); + m_lastError.clear(); + + if (!range.isValid()) { + m_lastError = "Invalid XYZ point-cloud range"; + LOG_ERROR("%s\n", m_lastError.c_str()); + if (finishedCallback) { + finishedCallback(ERR_CODE(FILE_ERR_FORMAT), 0, 0.0f, 0, 0); + } + return ERR_CODE(FILE_ERR_FORMAT); + } + + int lineNum = 0; + float scanSpeed = 0.0f; + int maxTimeStamp = 0; + int clockPerSecond = 0; + + auto finish = [&](int result) -> int { + if (finishedCallback) { + finishedCallback(result, lineNum, scanSpeed, maxTimeStamp, clockPerSecond); + } + return result; + }; + + if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) { + return _LoadLaserScanDataFromDatFileByIndexedCallback( + fileName, lineCallback, finishedCallback, range); + } + + 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 finish(ERR_CODE(FILE_ERR_NOEXIST)); + } + + EVzResultDataType dataType = keResultDataType_Invalid; + int result = _GetLaserType(fileName, dataType); + if (result != SUCCESS) { + m_lastError = "Cannot determine laser data type: " + fileName; + LOG_ERROR("%s\n", m_lastError.c_str()); + return finish(result); + } + + SVzLaserLineData lineData; + std::memset(&lineData, 0, sizeof(lineData)); + std::unique_ptr positionPoints; + std::unique_ptr position2DPoints; + std::unique_ptr rgbaPoints; + std::unique_ptr rgba2DPoints; + bool hasLine = false; + int pointIndex = 0; + + auto emitLine = [&]() -> int { + if (!hasLine) { + return SUCCESS; + } + if (pointIndex != lineData.nPointCount) { + std::ostringstream message; + message << "Laser line " << lineData.llFrameIdx << " declares " + << lineData.nPointCount << " points but contains " << pointIndex; + m_lastError = message.str(); + LOG_ERROR("%s\n", m_lastError.c_str()); + return ERR_CODE(FILE_ERR_FORMAT); + } + LaserLineLoadInfo loadInfo; + loadInfo.sourcePointCount = lineData.nPointCount; + FilterLaserLineInPlace(lineData, dataType, loadInfo.sourcePointCount, range, + &loadInfo.sourcePointIndices); + if (lineCallback) { + lineCallback(dataType, lineData, loadInfo); + } + positionPoints.reset(); + position2DPoints.reset(); + rgbaPoints.reset(); + rgba2DPoints.reset(); + std::memset(&lineData, 0, sizeof(lineData)); + hasLine = false; + pointIndex = 0; + return SUCCESS; + }; + + std::string line; + while (std::getline(inputFile, line)) { + TrimCarriageReturn(line); + + if (line.find("LineNum:") == 0) { + sscanf(line.c_str(), "LineNum:%d", &lineNum); + } else if (line.find("ScanSpeed:") == 0) { + sscanf(line.c_str(), "ScanSpeed:%f", &scanSpeed); + } else if (line.find("MaxTimeStamp:") == 0) { + sscanf(line.c_str(), "MaxTimeStamp:%d_%d", &maxTimeStamp, &clockPerSecond); + } else if (line.find("Line_") == 0) { + result = emitLine(); + if (result != SUCCESS) { + return finish(result); + } + + int lineIndex = 0; + unsigned int timeStamp = 0; + int pointCount = 0; + if (sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &pointCount) != 3 || + pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) { + m_lastError = "Invalid laser line header: " + line; + LOG_ERROR("%s\n", m_lastError.c_str()); + return finish(ERR_CODE(FILE_ERR_FORMAT)); + } + + lineData.llFrameIdx = lineIndex; + lineData.llTimeStamp = timeStamp; + lineData.nPointCount = pointCount; + + if (pointCount > 0 && dataType == keResultDataType_PointXYZRGBA) { + rgbaPoints.reset(new (std::nothrow) SVzNLPointXYZRGBA[pointCount]); + rgba2DPoints.reset(new (std::nothrow) SVzNL2DLRPoint[pointCount]); + if (!rgbaPoints || !rgba2DPoints) { + m_lastError = "Memory allocation failed while reading: " + fileName; + LOG_ERROR("%s\n", m_lastError.c_str()); + return finish(ERR_CODE(DATA_ERR_MEM)); + } + std::memset(rgbaPoints.get(), 0, + sizeof(SVzNLPointXYZRGBA) * static_cast(pointCount)); + std::memset(rgba2DPoints.get(), 0, + sizeof(SVzNL2DLRPoint) * static_cast(pointCount)); + lineData.p3DPoint = rgbaPoints.get(); + lineData.p2DPoint = rgba2DPoints.get(); + } else if (pointCount > 0 && dataType == keResultDataType_Position) { + positionPoints.reset(new (std::nothrow) SVzNL3DPosition[pointCount]); + position2DPoints.reset(new (std::nothrow) SVzNL2DPosition[pointCount]); + if (!positionPoints || !position2DPoints) { + m_lastError = "Memory allocation failed while reading: " + fileName; + LOG_ERROR("%s\n", m_lastError.c_str()); + return finish(ERR_CODE(DATA_ERR_MEM)); + } + std::memset(positionPoints.get(), 0, + sizeof(SVzNL3DPosition) * static_cast(pointCount)); + std::memset(position2DPoints.get(), 0, + sizeof(SVzNL2DPosition) * static_cast(pointCount)); + lineData.p3DPoint = positionPoints.get(); + lineData.p2DPoint = position2DPoints.get(); + } + hasLine = true; + if (pointCount == 0) { + result = emitLine(); + if (result != SUCCESS) { + return finish(result); + } + } + } else if (IsShapeBlockHeader(line)) { + const int shapePointCount = ShapeBlockPointCount(line); + for (int skipped = 0; skipped < shapePointCount;) { + if (!std::getline(inputFile, line)) { + break; + } + TrimCarriageReturn(line); + if (!line.empty()) { + ++skipped; + } + } + } else if (line.find("{") == 0) { + if (!hasLine || pointIndex >= lineData.nPointCount || + !lineData.p3DPoint || !lineData.p2DPoint) { + m_lastError = "Point data does not match laser line header: " + line; + LOG_ERROR("%s\n", m_lastError.c_str()); + return finish(ERR_CODE(FILE_ERR_FORMAT)); + } + + if (dataType == keResultDataType_PointXYZRGBA) { + result = _ParseLaserScanPoint(line, rgbaPoints[pointIndex], rgba2DPoints[pointIndex]); + } else { + result = _ParseLaserScanPoint(line, positionPoints[pointIndex], position2DPoints[pointIndex]); + positionPoints[pointIndex].nPointIdx = pointIndex; + } + if (result != SUCCESS) { + m_lastError = "Invalid point line: " + line; + LOG_ERROR("%s\n", m_lastError.c_str()); + return finish(result); + } + ++pointIndex; + if (pointIndex == lineData.nPointCount) { + result = emitLine(); + if (result != SUCCESS) { + return finish(result); + } + } + } + } + + result = emitLine(); + if (result != SUCCESS) { + return finish(result); + } + LOG_INFO("Successfully loaded %d laser scan lines by callback from file: %s\n", + lineNum, fileName.c_str()); + return finish(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, + const PointCloudXYZRange& range) +{ + 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; + + 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(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; + + 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(); + } + + FilterLaserLineInPlace(lineData, keResultDataType_Position, pointCount, range, nullptr); + + 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, + 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(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())); + } + } + } + + sw.flush(); + if (!sw.good()) { + m_lastError = "Failed while writing file: " + fileName; + LOG_ERROR("Failed while writing file: %s\n", fileName.c_str()); + return ERR_CODE(FILE_ERR_WRITE); + } + 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 > xyzData) +{ + return DebugSaveLaser(std::move(fileName), std::move(xyzData), nullptr); +} + +int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector> 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(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())); + } + + ++index; + if (callback) { + callback->onSaveProgress(static_cast(index) / static_cast(totalLines)); + } + } + + sw.flush(); + if (!sw.good()) { + m_lastError = "Failed while writing file: " + fileName; + LOG_ERROR("Failed while writing file: %s\n", fileName.c_str()); + return ERR_CODE(FILE_ERR_WRITE); + } + 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::LoadShapeBlocks(const std::string& fileName, + std::vector>& shapeBlocks) +{ + LOG_INFO("Loading shape blocks from file: %s\n", fileName.c_str()); + + shapeBlocks.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; + } + + // 查找图形块头。 + if (!IsShapeBlockHeader(line)) { + continue; + } + + // 解析点数量（最后一个下划线后的数字） + const int num = ShapeBlockPointCount(line); + if (num < 2) { + LOG_WARN("[LaserDataLoader] Invalid shape header: %s\n", line.c_str()); + continue; + } + + // 读取 num 个点坐标 + std::vector points; + bool incompleteBlock = false; + for (int i = 0; i < num; ) { + if (!std::getline(inputFile, line)) { + incompleteBlock = true; + 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(A) << 24; + nRGB |= static_cast(B) << 16; + nRGB |= static_cast(G) << 8; + nRGB |= static_cast(R); + pt.nRGB = nRGB; + } + parsed = true; + } + + // Compact coloured shape format: {x,y,z} - {R,G,B,A}. + if (!parsed) { + int cr = 255, cg = 255, cb = 255, ca = 255; + if (sscanf(line.c_str(), + " { %f , %f , %f } - { %d , %d , %d , %d }", + &fx, &fy, &fz, &cr, &cg, &cb, &ca) == 7) { + pt.x = fx; + pt.y = fy; + pt.z = fz; + pt.nRGB = (static_cast(ca) << 24) | + (static_cast(cb) << 16) | + (static_cast(cg) << 8) | + static_cast(cr); + 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 (incompleteBlock || points.size() != static_cast(num)) { + m_lastError = "Incomplete or invalid shape block in file: " + fileName; + LOG_ERROR("%s\n", m_lastError.c_str()); + shapeBlocks.clear(); + return ERR_CODE(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT); + } + shapeBlocks.push_back(std::move(points)); + } + + if (inputFile.bad()) { + m_lastError = "Failed while reading shape file: " + fileName; + LOG_ERROR("%s\n", m_lastError.c_str()); + shapeBlocks.clear(); + return ERR_CODE(FILE_ERR_READ); + } + + inputFile.close(); + LOG_INFO("Loaded %zu shape blocks from file: %s\n", shapeBlocks.size(), fileName.c_str()); + return SUCCESS; +} + +int LaserDataLoader::LoadPolySegments( + const std::string& fileName, + std::vector>& polyLines) +{ + return LoadShapeBlocks(fileName, polyLines); +} + +void LaserDataLoader::FreeLaserScanData(std::vector>& 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(lineData.p3DPoint); + break; + case keResultDataType_PositionF: + 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; + } + } + laserLines.clear(); + } + + LOG_DEBUG("Unified laser scan data freed successfully\n"); +} + +// 转换统一格式数据为std::vector>格式 +int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector>& unifiedData, + std::vector>& scanLines, + size_t* validPointCount) +{ + 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; + + if (lineData.nPointCount < 0) { + continue; + } + + // 只处理Position类型的数据；过滤后为空的扫描线也要保留位置。 + if ((dataType == keResultDataType_Position || + dataType == keResultDataType_PositionF) && + (lineData.p3DPoint || lineData.nPointCount == 0)) { + std::vector scanLine; + scanLine.reserve(lineData.nPointCount); + + const SVzNL3DPosition* points = static_cast(lineData.p3DPoint); + for (int i = 0; i < lineData.nPointCount; ++i) { + if (points == nullptr) { + break; + } + 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; +} + +int LaserDataLoader::ConvertToSVzNLPositionD( + 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) +{ + xyzData.clear(); + + for (const auto& linePair : unifiedData) { + EVzResultDataType dataType = linePair.first; + const SVzLaserLineData& lineData = linePair.second; + + // PositionF 的 3D buffer 也是 SVzNL3DPosition；仅 2D UV 为浮点。 + if ((dataType == keResultDataType_Position || + dataType == keResultDataType_PositionF) && + (lineData.p3DPoint || lineData.nPointCount == 0)) { + 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>& unifiedData, + std::vector& 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 || lineData.nPointCount == 0)) { + 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& 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& 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 = 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::_LoadLaserScanDataFromDatFileByIndexedCallback( + const std::string& fileName, + const LaserLineLoadIndexedCallback& lineCallback, + const LaserLoadFinishedCallback& finishedCallback, + const PointCloudXYZRange& range) +{ + int lineNum = 0; + float scanSpeed = 0.0f; + int maxTimeStamp = 0; + int clockPerSecond = 0; + + auto finish = [&](int result) -> int { + if (finishedCallback) { + finishedCallback(result, lineNum, scanSpeed, maxTimeStamp, clockPerSecond); + } + return result; + }; + + 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 finish(ERR_CODE(FILE_ERR_NOEXIST)); + } + + auto fail = [&](int code, const std::string& message) -> int { + m_lastError = message; + LOG_ERROR("%s\n", message.c_str()); + return finish(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 featurePointSize = 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) || + pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) { + return false; + } + pos += static_cast(sizeof(pointCount)); + const std::streamoff pointBytes = + static_cast(pointCount) * featurePointSize; + 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; + 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); + } + + std::unique_ptr points3D; + std::unique_ptr 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(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; + 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); + } + + 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); + lineData.p3DPoint = points3D.get(); + lineData.p2DPoint = points2D.get(); + LaserLineLoadInfo loadInfo; + loadInfo.sourcePointCount = pointCount; + FilterLaserLineInPlace(lineData, keResultDataType_Position, pointCount, range, + &loadInfo.sourcePointIndices); + if (lineCallback) { + lineCallback(keResultDataType_Position, lineData, loadInfo); + } + } + + LOG_INFO("Successfully loaded %d dat laser scan lines by callback from file: %s\n", + lineNum, fileName.c_str()); + return finish(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 = 0.0f, Y = 0.0f, Z = 0.0f; + float leftX = 0.0f, leftY = 0.0f; + float rightX = 0.0f, rightY = 0.0f; + + // 尝试新格式: {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(A) << 24; + nRGB |= static_cast(B) << 16; + nRGB |= static_cast(G) << 8; + nRGB |= static_cast(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 = 0.0f, g = 0.0f, b = 0.0f; + if (10 != sscanf(data.c_str(), + " { %f , %f , %f , %f , %f , %f } - { %f , %f } - { %f , %f }", + &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY)) { + return ERR_CODE(FILE_ERR_FORMAT); + } + 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) { + // XYZUV文本格式: {x,y,z}-{lx,ly}-{rx,ry} + eDataType = keResultDataType_Position; + bFind = true; + } + } + } + break; + } + } + + inputFile.close(); + return bFind ? SUCCESS : ERR_CODE(FILE_ERR_FORMAT); +} diff --git a/CloudUtils/Src/PointCloudFilter.cpp b/CloudUtils/Src/PointCloudFilter.cpp index c9d173c..3308199 100644 --- a/CloudUtils/Src/PointCloudFilter.cpp +++ b/CloudUtils/Src/PointCloudFilter.cpp @@ -1,383 +1,383 @@ -#include "PointCloudFilter.h" - -#include -#include -#include -#include -#include - -namespace -{ -struct GridKey -{ - long long x; - long long y; - long long z; - - bool operator==(const GridKey& other) const - { - return x == other.x && y == other.y && z == other.z; - } -}; - -struct GridKeyHash -{ - std::size_t operator()(const GridKey& key) const - { - const std::size_t h1 = std::hash()(key.x); - const std::size_t h2 = std::hash()(key.y); - const std::size_t h3 = std::hash()(key.z); - return h1 ^ (h2 + static_cast(0x9e3779b9) + (h1 << 6) + (h1 >> 2)) - ^ (h3 + static_cast(0x9e3779b9) + (h2 << 6) + (h2 >> 2)); - } -}; - -struct FlatPoint -{ - SVzNL3DPosition point; -}; - -bool IsFinite(const SVzNL3DPosition& point) -{ - return std::isfinite(point.pt3D.x) && std::isfinite(point.pt3D.y) - && std::isfinite(point.pt3D.z); -} - -double SquaredDistance(const SVzNL3DPosition& first, const SVzNL3DPosition& second) -{ - const double dx = first.pt3D.x - second.pt3D.x; - const double dy = first.pt3D.y - second.pt3D.y; - const double dz = first.pt3D.z - second.pt3D.z; - return dx * dx + dy * dy + dz * dz; -} - -long long ToCell(double value, double cellSize) -{ - const double coordinate = std::floor(value / cellSize); - if (coordinate <= static_cast(std::numeric_limits::min())) { - return std::numeric_limits::min(); - } - if (coordinate >= static_cast(std::numeric_limits::max())) { - return std::numeric_limits::max(); - } - return static_cast(coordinate); -} - -GridKey GetKey(const SVzNL3DPosition& point, double cellSize) -{ - GridKey key; - key.x = ToCell(point.pt3D.x, cellSize); - key.y = ToCell(point.pt3D.y, cellSize); - key.z = ToCell(point.pt3D.z, cellSize); - return key; -} - -void ResetStats(PointCloudFilterStats* stats, std::size_t inputCount) -{ - if (stats != nullptr) { - stats->inputPointCount = inputCount; - stats->outputPointCount = 0; - stats->removedPointCount = 0; - } -} - -void FinishStats(PointCloudFilterStats* stats, std::size_t outputCount) -{ - if (stats != nullptr) { - stats->outputPointCount = outputCount; - stats->removedPointCount = stats->inputPointCount >= outputCount - ? stats->inputPointCount - outputCount : 0; - } -} - -bool FilterScanLine(const std::vector& input, - std::vector& output, - const PointCloudFilterParams& params) -{ - output.clear(); - if (input.empty()) { - return true; - } - - const std::size_t minLength = std::max(1, params.minSegmentLength); - std::size_t segmentBegin = 0; - for (std::size_t index = 1; index <= input.size(); ++index) { - bool connected = index < input.size() && IsFinite(input[index - 1]) - && IsFinite(input[index]) - && std::abs(input[index].pt3D.z - input[index - 1].pt3D.z) - <= std::max(0.0, params.continuityThreshold); - if (index == input.size() || !connected) { - if (index - segmentBegin >= minLength) { - output.insert(output.end(), input.begin() + static_cast(segmentBegin), - input.begin() + static_cast(index)); - } - segmentBegin = index; - } - } - return true; -} - -double EstimateCellSize(const std::vector& points) -{ - if (points.size() < 2) { - return 1.0; - } - double minX = points[0].point.pt3D.x; - double maxX = minX; - double minY = points[0].point.pt3D.y; - double maxY = minY; - double minZ = points[0].point.pt3D.z; - double maxZ = minZ; - for (std::size_t index = 1; index < points.size(); ++index) { - const SVzNL3DPoint& value = points[index].point.pt3D; - minX = std::min(minX, value.x); - maxX = std::max(maxX, value.x); - minY = std::min(minY, value.y); - maxY = std::max(maxY, value.y); - minZ = std::min(minZ, value.z); - maxZ = std::max(maxZ, value.z); - } - const double diagonal = std::sqrt((maxX - minX) * (maxX - minX) - + (maxY - minY) * (maxY - minY) + (maxZ - minZ) * (maxZ - minZ)); - const double scale = diagonal / std::cbrt(static_cast(points.size())); - return std::max(scale, 1e-6); -} - -template -void FilterWithGrid(const std::vector& points, double cellSize, - double radius, KeepPredicate keepPoint, - std::vector& keep) -{ - std::unordered_map, GridKeyHash> grid; - grid.reserve(points.size() * 2 + 1); - for (std::size_t index = 0; index < points.size(); ++index) { - grid[GetKey(points[index].point, cellSize)].push_back(index); - } - - const long long cellRange = std::max(1, static_cast(std::ceil(radius / cellSize))); - const double radiusSquared = radius * radius; - std::vector candidates; - candidates.reserve(32); - for (std::size_t index = 0; index < points.size(); ++index) { - const GridKey center = GetKey(points[index].point, cellSize); - candidates.clear(); - for (long long x = -cellRange; x <= cellRange; ++x) { - for (long long y = -cellRange; y <= cellRange; ++y) { - for (long long z = -cellRange; z <= cellRange; ++z) { - GridKey key = {center.x + x, center.y + y, center.z + z}; - const auto found = grid.find(key); - if (found != grid.end()) { - candidates.insert(candidates.end(), found->second.begin(), found->second.end()); - } - } - } - } - keep[index] = keepPoint(index, candidates, radiusSquared); - } -} -} - -bool PointCloudFilter::Filter(const std::vector>& input, - std::vector>& output, - const PointCloudFilterParams& params, - PointCloudFilterStats* stats) -{ - std::size_t inputCount = 0; - for (const auto& line : input) { - inputCount += line.size(); - } - ResetStats(stats, inputCount); - output.clear(); - output.resize(input.size()); - - if (params.method == PointCloudFilterMethod::None) { - output = input; - FinishStats(stats, inputCount); - return true; - } - if (params.method == PointCloudFilterMethod::ScanLineContinuity) { - std::size_t outputCount = 0; - for (std::size_t lineIndex = 0; lineIndex < input.size(); ++lineIndex) { - FilterScanLine(input[lineIndex], output[lineIndex], params); - outputCount += output[lineIndex].size(); - } - FinishStats(stats, outputCount); - return true; - } - if (params.method != PointCloudFilterMethod::RadiusOutlier - && params.method != PointCloudFilterMethod::StatisticalOutlier) { - output.clear(); - return false; - } - if (params.method == PointCloudFilterMethod::RadiusOutlier && params.radius <= 0.0) { - output.clear(); - return false; - } - - std::vector points; - points.reserve(inputCount); - for (const auto& line : input) { - for (const SVzNL3DPosition& point : line) { - if (IsFinite(point)) { - points.push_back({point}); - } - } - } - std::vector filtered; - std::vector flatInput; - flatInput.reserve(points.size()); - for (std::size_t index = 0; index < points.size(); ++index) { - flatInput.push_back(points[index].point); - // Encode the flattened position only for reconstructing scan lines. - flatInput.back().nPointIdx = static_cast(index); - } - if (!Filter(flatInput, filtered, params, nullptr)) { - output.clear(); - return false; - } - - std::vector retained(points.size(), false); - for (const SVzNL3DPosition& point : filtered) { - if (point.nPointIdx >= 0 - && static_cast(point.nPointIdx) < retained.size()) { - retained[static_cast(point.nPointIdx)] = true; - } - } - std::size_t pointIndex = 0; - for (std::size_t lineIndex = 0; lineIndex < input.size(); ++lineIndex) { - for (const SVzNL3DPosition& point : input[lineIndex]) { - if (IsFinite(point)) { - if (pointIndex < retained.size() && retained[pointIndex]) { - output[lineIndex].push_back(point); - } - ++pointIndex; - } - } - } - std::size_t outputCount = 0; - for (const auto& line : output) { - outputCount += line.size(); - } - FinishStats(stats, outputCount); - return true; -} - -bool PointCloudFilter::Filter(const std::vector& input, - std::vector& output, - const PointCloudFilterParams& params, - PointCloudFilterStats* stats) -{ - ResetStats(stats, input.size()); - output.clear(); - if (params.method == PointCloudFilterMethod::None) { - output = input; - FinishStats(stats, output.size()); - return true; - } - if (params.method == PointCloudFilterMethod::ScanLineContinuity) { - FilterScanLine(input, output, params); - FinishStats(stats, output.size()); - return true; - } - if (params.method != PointCloudFilterMethod::RadiusOutlier - && params.method != PointCloudFilterMethod::StatisticalOutlier) { - return false; - } - if (params.method == PointCloudFilterMethod::RadiusOutlier && params.radius <= 0.0) { - return false; - } - - std::vector points; - points.reserve(input.size()); - for (const SVzNL3DPosition& point : input) { - if (IsFinite(point)) { - points.push_back({point}); - } - } - if (points.empty()) { - FinishStats(stats, 0); - return true; - } - - const double requestedRadius = params.method == PointCloudFilterMethod::RadiusOutlier - ? params.radius : params.searchRadius; - const double radius = requestedRadius > 0.0 ? requestedRadius : EstimateCellSize(points) * 2.0; - const double cellSize = std::max(radius, 1e-6); - std::vector keep(points.size(), false); - - if (params.method == PointCloudFilterMethod::RadiusOutlier) { - const std::size_t minNeighbors = params.minNeighbors; - FilterWithGrid(points, cellSize, radius, - [&points, minNeighbors](std::size_t index, const std::vector& candidates, - double radiusSquared) { - std::size_t neighbors = 0; - for (std::size_t candidate : candidates) { - if (candidate != index && SquaredDistance(points[index].point, points[candidate].point) <= radiusSquared) { - ++neighbors; - } - } - return neighbors >= minNeighbors; - }, keep); - } - - if (params.method == PointCloudFilterMethod::StatisticalOutlier) { - std::vector meanDistances(points.size(), std::numeric_limits::infinity()); - FilterWithGrid(points, cellSize, radius, - [&points, &meanDistances, ¶ms](std::size_t index, - const std::vector& candidates, - double radiusSquared) { - std::vector distances; - distances.reserve(candidates.size()); - for (std::size_t candidate : candidates) { - if (candidate != index) { - const double distance = SquaredDistance(points[index].point, points[candidate].point); - if (distance <= radiusSquared) { - distances.push_back(std::sqrt(distance)); - } - } - } - if (distances.empty()) { - return false; - } - const std::size_t count = std::min(params.meanK == 0 ? std::size_t(1) : params.meanK, - distances.size()); - if (count < distances.size()) { - std::nth_element(distances.begin(), - distances.begin() + static_cast(count), - distances.end()); - } - meanDistances[index] = std::accumulate(distances.begin(), distances.begin() + static_cast(count), 0.0) - / static_cast(count); - return true; - }, keep); - std::vector validMeans; - for (double value : meanDistances) { - if (std::isfinite(value)) { - validMeans.push_back(value); - } - } - if (!validMeans.empty()) { - const double mean = std::accumulate(validMeans.begin(), validMeans.end(), 0.0) - / static_cast(validMeans.size()); - double variance = 0.0; - for (double value : validMeans) { - const double delta = value - mean; - variance += delta * delta; - } - const double deviation = std::sqrt(variance / static_cast(validMeans.size())); - const double threshold = mean + std::max(0.0, params.standardDeviationMultiplier) * deviation; - for (std::size_t index = 0; index < keep.size(); ++index) { - keep[index] = std::isfinite(meanDistances[index]) && meanDistances[index] <= threshold; - } - } - } - - output.reserve(points.size()); - for (std::size_t index = 0; index < points.size(); ++index) { - if (keep[index]) { - output.push_back(points[index].point); - } - } - FinishStats(stats, output.size()); - return true; -} +#include "PointCloudFilter.h" + +#include +#include +#include +#include +#include + +namespace +{ +struct GridKey +{ + long long x; + long long y; + long long z; + + bool operator==(const GridKey& other) const + { + return x == other.x && y == other.y && z == other.z; + } +}; + +struct GridKeyHash +{ + std::size_t operator()(const GridKey& key) const + { + const std::size_t h1 = std::hash()(key.x); + const std::size_t h2 = std::hash()(key.y); + const std::size_t h3 = std::hash()(key.z); + return h1 ^ (h2 + static_cast(0x9e3779b9) + (h1 << 6) + (h1 >> 2)) + ^ (h3 + static_cast(0x9e3779b9) + (h2 << 6) + (h2 >> 2)); + } +}; + +struct FlatPoint +{ + SVzNL3DPosition point; +}; + +bool IsFinite(const SVzNL3DPosition& point) +{ + return std::isfinite(point.pt3D.x) && std::isfinite(point.pt3D.y) + && std::isfinite(point.pt3D.z); +} + +double SquaredDistance(const SVzNL3DPosition& first, const SVzNL3DPosition& second) +{ + const double dx = first.pt3D.x - second.pt3D.x; + const double dy = first.pt3D.y - second.pt3D.y; + const double dz = first.pt3D.z - second.pt3D.z; + return dx * dx + dy * dy + dz * dz; +} + +long long ToCell(double value, double cellSize) +{ + const double coordinate = std::floor(value / cellSize); + if (coordinate <= static_cast(std::numeric_limits::min())) { + return std::numeric_limits::min(); + } + if (coordinate >= static_cast(std::numeric_limits::max())) { + return std::numeric_limits::max(); + } + return static_cast(coordinate); +} + +GridKey GetKey(const SVzNL3DPosition& point, double cellSize) +{ + GridKey key; + key.x = ToCell(point.pt3D.x, cellSize); + key.y = ToCell(point.pt3D.y, cellSize); + key.z = ToCell(point.pt3D.z, cellSize); + return key; +} + +void ResetStats(PointCloudFilterStats* stats, std::size_t inputCount) +{ + if (stats != nullptr) { + stats->inputPointCount = inputCount; + stats->outputPointCount = 0; + stats->removedPointCount = 0; + } +} + +void FinishStats(PointCloudFilterStats* stats, std::size_t outputCount) +{ + if (stats != nullptr) { + stats->outputPointCount = outputCount; + stats->removedPointCount = stats->inputPointCount >= outputCount + ? stats->inputPointCount - outputCount : 0; + } +} + +bool FilterScanLine(const std::vector& input, + std::vector& output, + const PointCloudFilterParams& params) +{ + output.clear(); + if (input.empty()) { + return true; + } + + const std::size_t minLength = std::max(1, params.minSegmentLength); + std::size_t segmentBegin = 0; + for (std::size_t index = 1; index <= input.size(); ++index) { + bool connected = index < input.size() && IsFinite(input[index - 1]) + && IsFinite(input[index]) + && std::abs(input[index].pt3D.z - input[index - 1].pt3D.z) + <= std::max(0.0, params.continuityThreshold); + if (index == input.size() || !connected) { + if (index - segmentBegin >= minLength) { + output.insert(output.end(), input.begin() + static_cast(segmentBegin), + input.begin() + static_cast(index)); + } + segmentBegin = index; + } + } + return true; +} + +double EstimateCellSize(const std::vector& points) +{ + if (points.size() < 2) { + return 1.0; + } + double minX = points[0].point.pt3D.x; + double maxX = minX; + double minY = points[0].point.pt3D.y; + double maxY = minY; + double minZ = points[0].point.pt3D.z; + double maxZ = minZ; + for (std::size_t index = 1; index < points.size(); ++index) { + const SVzNL3DPoint& value = points[index].point.pt3D; + minX = std::min(minX, value.x); + maxX = std::max(maxX, value.x); + minY = std::min(minY, value.y); + maxY = std::max(maxY, value.y); + minZ = std::min(minZ, value.z); + maxZ = std::max(maxZ, value.z); + } + const double diagonal = std::sqrt((maxX - minX) * (maxX - minX) + + (maxY - minY) * (maxY - minY) + (maxZ - minZ) * (maxZ - minZ)); + const double scale = diagonal / std::cbrt(static_cast(points.size())); + return std::max(scale, 1e-6); +} + +template +void FilterWithGrid(const std::vector& points, double cellSize, + double radius, KeepPredicate keepPoint, + std::vector& keep) +{ + std::unordered_map, GridKeyHash> grid; + grid.reserve(points.size() * 2 + 1); + for (std::size_t index = 0; index < points.size(); ++index) { + grid[GetKey(points[index].point, cellSize)].push_back(index); + } + + const long long cellRange = std::max(1, static_cast(std::ceil(radius / cellSize))); + const double radiusSquared = radius * radius; + std::vector candidates; + candidates.reserve(32); + for (std::size_t index = 0; index < points.size(); ++index) { + const GridKey center = GetKey(points[index].point, cellSize); + candidates.clear(); + for (long long x = -cellRange; x <= cellRange; ++x) { + for (long long y = -cellRange; y <= cellRange; ++y) { + for (long long z = -cellRange; z <= cellRange; ++z) { + GridKey key = {center.x + x, center.y + y, center.z + z}; + const auto found = grid.find(key); + if (found != grid.end()) { + candidates.insert(candidates.end(), found->second.begin(), found->second.end()); + } + } + } + } + keep[index] = keepPoint(index, candidates, radiusSquared); + } +} +} + +bool PointCloudFilter::Filter(const std::vector>& input, + std::vector>& output, + const PointCloudFilterParams& params, + PointCloudFilterStats* stats) +{ + std::size_t inputCount = 0; + for (const auto& line : input) { + inputCount += line.size(); + } + ResetStats(stats, inputCount); + output.clear(); + output.resize(input.size()); + + if (params.method == PointCloudFilterMethod::None) { + output = input; + FinishStats(stats, inputCount); + return true; + } + if (params.method == PointCloudFilterMethod::ScanLineContinuity) { + std::size_t outputCount = 0; + for (std::size_t lineIndex = 0; lineIndex < input.size(); ++lineIndex) { + FilterScanLine(input[lineIndex], output[lineIndex], params); + outputCount += output[lineIndex].size(); + } + FinishStats(stats, outputCount); + return true; + } + if (params.method != PointCloudFilterMethod::RadiusOutlier + && params.method != PointCloudFilterMethod::StatisticalOutlier) { + output.clear(); + return false; + } + if (params.method == PointCloudFilterMethod::RadiusOutlier && params.radius <= 0.0) { + output.clear(); + return false; + } + + std::vector points; + points.reserve(inputCount); + for (const auto& line : input) { + for (const SVzNL3DPosition& point : line) { + if (IsFinite(point)) { + points.push_back({point}); + } + } + } + std::vector filtered; + std::vector flatInput; + flatInput.reserve(points.size()); + for (std::size_t index = 0; index < points.size(); ++index) { + flatInput.push_back(points[index].point); + // Encode the flattened position only for reconstructing scan lines. + flatInput.back().nPointIdx = static_cast(index); + } + if (!Filter(flatInput, filtered, params, nullptr)) { + output.clear(); + return false; + } + + std::vector retained(points.size(), false); + for (const SVzNL3DPosition& point : filtered) { + if (point.nPointIdx >= 0 + && static_cast(point.nPointIdx) < retained.size()) { + retained[static_cast(point.nPointIdx)] = true; + } + } + std::size_t pointIndex = 0; + for (std::size_t lineIndex = 0; lineIndex < input.size(); ++lineIndex) { + for (const SVzNL3DPosition& point : input[lineIndex]) { + if (IsFinite(point)) { + if (pointIndex < retained.size() && retained[pointIndex]) { + output[lineIndex].push_back(point); + } + ++pointIndex; + } + } + } + std::size_t outputCount = 0; + for (const auto& line : output) { + outputCount += line.size(); + } + FinishStats(stats, outputCount); + return true; +} + +bool PointCloudFilter::Filter(const std::vector& input, + std::vector& output, + const PointCloudFilterParams& params, + PointCloudFilterStats* stats) +{ + ResetStats(stats, input.size()); + output.clear(); + if (params.method == PointCloudFilterMethod::None) { + output = input; + FinishStats(stats, output.size()); + return true; + } + if (params.method == PointCloudFilterMethod::ScanLineContinuity) { + FilterScanLine(input, output, params); + FinishStats(stats, output.size()); + return true; + } + if (params.method != PointCloudFilterMethod::RadiusOutlier + && params.method != PointCloudFilterMethod::StatisticalOutlier) { + return false; + } + if (params.method == PointCloudFilterMethod::RadiusOutlier && params.radius <= 0.0) { + return false; + } + + std::vector points; + points.reserve(input.size()); + for (const SVzNL3DPosition& point : input) { + if (IsFinite(point)) { + points.push_back({point}); + } + } + if (points.empty()) { + FinishStats(stats, 0); + return true; + } + + const double requestedRadius = params.method == PointCloudFilterMethod::RadiusOutlier + ? params.radius : params.searchRadius; + const double radius = requestedRadius > 0.0 ? requestedRadius : EstimateCellSize(points) * 2.0; + const double cellSize = std::max(radius, 1e-6); + std::vector keep(points.size(), false); + + if (params.method == PointCloudFilterMethod::RadiusOutlier) { + const std::size_t minNeighbors = params.minNeighbors; + FilterWithGrid(points, cellSize, radius, + [&points, minNeighbors](std::size_t index, const std::vector& candidates, + double radiusSquared) { + std::size_t neighbors = 0; + for (std::size_t candidate : candidates) { + if (candidate != index && SquaredDistance(points[index].point, points[candidate].point) <= radiusSquared) { + ++neighbors; + } + } + return neighbors >= minNeighbors; + }, keep); + } + + if (params.method == PointCloudFilterMethod::StatisticalOutlier) { + std::vector meanDistances(points.size(), std::numeric_limits::infinity()); + FilterWithGrid(points, cellSize, radius, + [&points, &meanDistances, ¶ms](std::size_t index, + const std::vector& candidates, + double radiusSquared) { + std::vector distances; + distances.reserve(candidates.size()); + for (std::size_t candidate : candidates) { + if (candidate != index) { + const double distance = SquaredDistance(points[index].point, points[candidate].point); + if (distance <= radiusSquared) { + distances.push_back(std::sqrt(distance)); + } + } + } + if (distances.empty()) { + return false; + } + const std::size_t count = std::min(params.meanK == 0 ? std::size_t(1) : params.meanK, + distances.size()); + if (count < distances.size()) { + std::nth_element(distances.begin(), + distances.begin() + static_cast(count), + distances.end()); + } + meanDistances[index] = std::accumulate(distances.begin(), distances.begin() + static_cast(count), 0.0) + / static_cast(count); + return true; + }, keep); + std::vector validMeans; + for (double value : meanDistances) { + if (std::isfinite(value)) { + validMeans.push_back(value); + } + } + if (!validMeans.empty()) { + const double mean = std::accumulate(validMeans.begin(), validMeans.end(), 0.0) + / static_cast(validMeans.size()); + double variance = 0.0; + for (double value : validMeans) { + const double delta = value - mean; + variance += delta * delta; + } + const double deviation = std::sqrt(variance / static_cast(validMeans.size())); + const double threshold = mean + std::max(0.0, params.standardDeviationMultiplier) * deviation; + for (std::size_t index = 0; index < keep.size(); ++index) { + keep[index] = std::isfinite(meanDistances[index]) && meanDistances[index] <= threshold; + } + } + } + + output.reserve(points.size()); + for (std::size_t index = 0; index < points.size(); ++index) { + if (keep[index]) { + output.push_back(points[index].point); + } + } + FinishStats(stats, output.size()); + return true; +} diff --git a/CloudView3D/CloudView3D.pro b/CloudView3D/CloudView3D.pro index 9d5fea8..e68059c 100644 --- a/CloudView3D/CloudView3D.pro +++ b/CloudView3D/CloudView3D.pro @@ -1,30 +1,30 @@ -QT += core gui widgets opengl - -CONFIG += c++17 staticlib -TEMPLATE = lib -TARGET = CloudView3D - -win32-msvc { - QMAKE_CXXFLAGS += /utf-8 - QMAKE_CXXFLAGS += /bigobj -} - -INCLUDEPATH += $$PWD/Inc -INCLUDEPATH += $$PWD/../VrUtils/Inc -INCLUDEPATH += $$PWD/../VrCommon/Inc -# VZNL_Types.h is maintained in the shared Utils module. - -HEADERS += \ - Inc/CloudView3D.h \ - Inc/CloudView3DTypes.h \ - Inc/PointCloudGLWidget.h - -SOURCES += \ - Src/PointCloudGLWidget.cpp - -win32 { - LIBS += -lopengl32 - LIBS += -lglu32 -} - -DEFINES += _USE_MATH_DEFINES +QT += core gui widgets opengl + +CONFIG += c++17 staticlib +TEMPLATE = lib +TARGET = CloudView3D + +win32-msvc { + QMAKE_CXXFLAGS += /utf-8 + QMAKE_CXXFLAGS += /bigobj +} + +INCLUDEPATH += $$PWD/Inc +INCLUDEPATH += $$PWD/../VrUtils/Inc +INCLUDEPATH += $$PWD/../VrCommon/Inc +# VZNL_Types.h is maintained in the shared Utils module. + +HEADERS += \ + Inc/CloudView3D.h \ + Inc/CloudView3DTypes.h \ + Inc/PointCloudGLWidget.h + +SOURCES += \ + Src/PointCloudGLWidget.cpp + +win32 { + LIBS += -lopengl32 + LIBS += -lglu32 +} + +DEFINES += _USE_MATH_DEFINES diff --git a/CloudView3D/Inc/CloudView3D.h b/CloudView3D/Inc/CloudView3D.h index f0fd68b..d8cf93a 100644 --- a/CloudView3D/Inc/CloudView3D.h +++ b/CloudView3D/Inc/CloudView3D.h @@ -1,7 +1,7 @@ -#ifndef CLOUD_VIEW_3D_H -#define CLOUD_VIEW_3D_H - -#include "CloudView3DTypes.h" -#include "PointCloudGLWidget.h" - -#endif // CLOUD_VIEW_3D_H +#ifndef CLOUD_VIEW_3D_H +#define CLOUD_VIEW_3D_H + +#include "CloudView3DTypes.h" +#include "PointCloudGLWidget.h" + +#endif // CLOUD_VIEW_3D_H diff --git a/CloudView3D/Inc/CloudView3DTypes.h b/CloudView3D/Inc/CloudView3DTypes.h index 88dde61..ea86037 100644 --- a/CloudView3D/Inc/CloudView3DTypes.h +++ b/CloudView3D/Inc/CloudView3DTypes.h @@ -1,77 +1,77 @@ -#ifndef CLOUD_VIEW_3D_TYPES_H -#define CLOUD_VIEW_3D_TYPES_H - -#include -#include -#include - -struct Point3D -{ - float x, y, z; // millimetres - Point3D() : x(0), y(0), z(0) {} - Point3D(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {} -}; - -struct Point3DRGB -{ - float x, y, z; // millimetres - uint8_t r, g, b; - float pointSize; - - Point3DRGB() : x(0), y(0), z(0), r(255), g(255), b(255), pointSize(0) {} - Point3DRGB(float _x, - float _y, - float _z, - uint8_t _r = 255, - uint8_t _g = 255, - uint8_t _b = 255, - float _ps = 0) - : x(_x), y(_y), z(_z), r(_r), g(_g), b(_b), pointSize(_ps) {} -}; - -template -class SimplePointCloud -{ -public: - std::vector points; - std::vector lineIndices; - // Each point's original position within its scan line (-1 when unknown). - std::vector pointInLineIndices; - - void clear() - { - points.clear(); - lineIndices.clear(); - pointInLineIndices.clear(); - } - size_t size() const { return points.size(); } - bool empty() const { return points.empty(); } - void reserve(size_t n) - { - points.reserve(n); - lineIndices.reserve(n); - pointInLineIndices.reserve(n); - } - void push_back(const PointT& pt) - { - points.push_back(pt); - pointInLineIndices.push_back(-1); - } - void push_back(const PointT& pt, int lineIdx) - { - points.push_back(pt); - lineIndices.push_back(lineIdx); - pointInLineIndices.push_back(-1); - } - void push_back(const PointT& pt, int lineIdx, int pointInLineIdx) - { - points.push_back(pt); - lineIndices.push_back(lineIdx); - pointInLineIndices.push_back(pointInLineIdx); - } -}; - -using PointCloudXYZ = SimplePointCloud; -using PointCloudXYZRGB = SimplePointCloud; - -#endif // CLOUD_VIEW_3D_TYPES_H +#ifndef CLOUD_VIEW_3D_TYPES_H +#define CLOUD_VIEW_3D_TYPES_H + +#include +#include +#include + +struct Point3D +{ + float x, y, z; // millimetres + Point3D() : x(0), y(0), z(0) {} + Point3D(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {} +}; + +struct Point3DRGB +{ + float x, y, z; // millimetres + uint8_t r, g, b; + float pointSize; + + Point3DRGB() : x(0), y(0), z(0), r(255), g(255), b(255), pointSize(0) {} + Point3DRGB(float _x, + float _y, + float _z, + uint8_t _r = 255, + uint8_t _g = 255, + uint8_t _b = 255, + float _ps = 0) + : x(_x), y(_y), z(_z), r(_r), g(_g), b(_b), pointSize(_ps) {} +}; + +template +class SimplePointCloud +{ +public: + std::vector points; + std::vector lineIndices; + // Each point's original position within its scan line (-1 when unknown). + std::vector pointInLineIndices; + + void clear() + { + points.clear(); + lineIndices.clear(); + pointInLineIndices.clear(); + } + size_t size() const { return points.size(); } + bool empty() const { return points.empty(); } + void reserve(size_t n) + { + points.reserve(n); + lineIndices.reserve(n); + pointInLineIndices.reserve(n); + } + void push_back(const PointT& pt) + { + points.push_back(pt); + pointInLineIndices.push_back(-1); + } + void push_back(const PointT& pt, int lineIdx) + { + points.push_back(pt); + lineIndices.push_back(lineIdx); + pointInLineIndices.push_back(-1); + } + void push_back(const PointT& pt, int lineIdx, int pointInLineIdx) + { + points.push_back(pt); + lineIndices.push_back(lineIdx); + pointInLineIndices.push_back(pointInLineIdx); + } +}; + +using PointCloudXYZ = SimplePointCloud; +using PointCloudXYZRGB = SimplePointCloud; + +#endif // CLOUD_VIEW_3D_TYPES_H diff --git a/CloudViewApp/Inc/CloudViewSettings.h b/CloudViewApp/Inc/CloudViewSettings.h index 575db42..ea948d5 100644 --- a/CloudViewApp/Inc/CloudViewSettings.h +++ b/CloudViewApp/Inc/CloudViewSettings.h @@ -1,27 +1,27 @@ -#ifndef CLOUD_VIEW_SETTINGS_H -#define CLOUD_VIEW_SETTINGS_H - -#include -#include - -#include "PointCloudXYZRange.h" - -struct CloudViewSettingsData -{ - float poseAxisLength = 10.0f; - bool pseudoColorEnabled = true; - float pseudoColorPeriod = 500.0f; - QVector3D pseudoColorStartColor = QVector3D(0.0f, 0.0f, 1.0f); - QVector3D pseudoColorEndColor = QVector3D(1.0f, 0.0f, 0.0f); - bool pseudoColorDefaultPalette = true; - PointCloudXYZRange pointCloudRange; -}; - -class CloudViewSettings -{ -public: - static CloudViewSettingsData load(); - static bool save(const CloudViewSettingsData& settings); -}; - -#endif // CLOUD_VIEW_SETTINGS_H +#ifndef CLOUD_VIEW_SETTINGS_H +#define CLOUD_VIEW_SETTINGS_H + +#include +#include + +#include "PointCloudXYZRange.h" + +struct CloudViewSettingsData +{ + float poseAxisLength = 10.0f; + bool pseudoColorEnabled = true; + float pseudoColorPeriod = 500.0f; + QVector3D pseudoColorStartColor = QVector3D(0.0f, 0.0f, 1.0f); + QVector3D pseudoColorEndColor = QVector3D(1.0f, 0.0f, 0.0f); + bool pseudoColorDefaultPalette = true; + PointCloudXYZRange pointCloudRange; +}; + +class CloudViewSettings +{ +public: + static CloudViewSettingsData load(); + static bool save(const CloudViewSettingsData& settings); +}; + +#endif // CLOUD_VIEW_SETTINGS_H diff --git a/CloudViewApp/Inc/PointCloudGLWidget.h b/CloudViewApp/Inc/PointCloudGLWidget.h index d54df38..c5b6e35 100644 --- a/CloudViewApp/Inc/PointCloudGLWidget.h +++ b/CloudViewApp/Inc/PointCloudGLWidget.h @@ -1,6 +1,6 @@ -#ifndef CLOUD_VIEW_POINT_CLOUD_GL_WIDGET_FORWARDER_H -#define CLOUD_VIEW_POINT_CLOUD_GL_WIDGET_FORWARDER_H - -#include "../../CloudView3D/Inc/PointCloudGLWidget.h" - -#endif // CLOUD_VIEW_POINT_CLOUD_GL_WIDGET_FORWARDER_H +#ifndef CLOUD_VIEW_POINT_CLOUD_GL_WIDGET_FORWARDER_H +#define CLOUD_VIEW_POINT_CLOUD_GL_WIDGET_FORWARDER_H + +#include "../../CloudView3D/Inc/PointCloudGLWidget.h" + +#endif // CLOUD_VIEW_POINT_CLOUD_GL_WIDGET_FORWARDER_H diff --git a/CloudViewApp/Src/CloudViewSettings.cpp b/CloudViewApp/Src/CloudViewSettings.cpp index f711b30..5794ca7 100644 --- a/CloudViewApp/Src/CloudViewSettings.cpp +++ b/CloudViewApp/Src/CloudViewSettings.cpp @@ -1,171 +1,171 @@ -#include "CloudViewSettings.h" - -#include -#include -#include -#include -#include - -namespace -{ -QString settingsFilePath() -{ - QString settingsDir = QStandardPaths::writableLocation(QStandardPaths::AppDataLocation); - if (settingsDir.isEmpty()) { - settingsDir = QDir::homePath() + "/CloudView"; - } - QDir().mkpath(settingsDir); - return QDir(settingsDir).filePath("CloudView.ini"); -} -} - -CloudViewSettingsData CloudViewSettings::load() -{ - CloudViewSettingsData result; - QSettings settings(settingsFilePath(), QSettings::IniFormat); - - // 模式独立于启用状态保存；缺失或非法配置默认采用“替换为全 0”。 - const QVariant savedModeValue = settings.contains("PointCloudRange/filterMode") - ? settings.value("PointCloudRange/filterMode") - : settings.value("PointCloudRange/mode", - static_cast(PointCloudXYZRangeFilterMode::ReplaceWithZero)); - const int savedFilterMode = savedModeValue.toInt(); - const PointCloudXYZRangeFilterMode filterMode = - savedFilterMode == static_cast(PointCloudXYZRangeFilterMode::Remove) - ? PointCloudXYZRangeFilterMode::Remove - : PointCloudXYZRangeFilterMode::ReplaceWithZero; - - const float poseAxisLength = settings.value("Display/poseAxisLength", result.poseAxisLength).toFloat(); - if (std::isfinite(poseAxisLength) && poseAxisLength > 0.0f) { - result.poseAxisLength = poseAxisLength; - } - - result.pseudoColorEnabled = settings.value("PseudoColor/enabled", result.pseudoColorEnabled).toBool(); - const bool hasPaletteSetting = settings.contains("PseudoColor/defaultPalette"); - result.pseudoColorDefaultPalette = hasPaletteSetting - ? settings.value("PseudoColor/defaultPalette").toBool() - : result.pseudoColorDefaultPalette; - const float period = settings.value("PseudoColor/period", result.pseudoColorPeriod).toFloat(); - if (std::isfinite(period) && period > 0.0f) { - result.pseudoColorPeriod = period; - } - const QColor startColor(settings.value("PseudoColor/startColor", "#0000ff").toString()); - const QColor endColor(settings.value("PseudoColor/endColor", "#ff0000").toString()); - if (startColor.isValid()) { - result.pseudoColorStartColor = QVector3D(startColor.redF(), startColor.greenF(), startColor.blueF()); - } - if (endColor.isValid()) { - result.pseudoColorEndColor = QVector3D(endColor.redF(), endColor.greenF(), endColor.blueF()); - } - - if (!hasPaletteSetting && settings.contains("PseudoColor/period")) { - const QColor legacyStartColor(settings.value("PseudoColor/startColor", "#0000ff").toString()); - const QColor legacyEndColor(settings.value("PseudoColor/endColor", "#ff0000").toString()); - const bool legacyDefaults = qFuzzyCompare(result.pseudoColorPeriod, 100.0f) && - legacyStartColor == QColor("#0000ff") && - legacyEndColor == QColor("#ff0000"); - if (legacyDefaults) { - // 旧版周期色环配置迁移为 Z 范围蓝→红梯度。 - result.pseudoColorPeriod = 500.0f; - result.pseudoColorStartColor = QVector3D(0.0f, 0.0f, 1.0f); - result.pseudoColorEndColor = QVector3D(1.0f, 0.0f, 0.0f); - result.pseudoColorDefaultPalette = true; - } else { - // 非默认旧配置继续使用原有的自定义起止色渐变。 - result.pseudoColorDefaultPalette = false; - } - } - - const bool rangeEnabled = settings.value("PointCloudRange/enabled", false).toBool(); - PointCloudXYZRange range; - range.enabled = rangeEnabled; - range.filterMode = filterMode; - if (rangeEnabled) { - const auto readBound = [&settings](const char* key, double fallback) { - bool ok = false; - const double value = settings.value(key).toDouble(&ok); - return ok && std::isfinite(value) ? value : fallback; - }; - // Missing bounds retain the range object's implicit unbounded side. - range.minX = readBound("PointCloudRange/minX", range.minX); - range.maxX = readBound("PointCloudRange/maxX", range.maxX); - range.minY = readBound("PointCloudRange/minY", range.minY); - range.maxY = readBound("PointCloudRange/maxY", range.maxY); - range.minZ = readBound("PointCloudRange/minZ", range.minZ); - range.maxZ = readBound("PointCloudRange/maxZ", range.maxZ); - if (range.isValid()) { - result.pointCloudRange = range; - } - } else { - result.pointCloudRange = range; - } - // Keep the mode even when a persisted enabled range is invalid and is - // therefore reset to the disabled default range. - result.pointCloudRange.filterMode = filterMode; - - return result; -} - -bool CloudViewSettings::save(const CloudViewSettingsData& value) -{ - QSettings settings(settingsFilePath(), QSettings::IniFormat); - if (std::isfinite(value.poseAxisLength) && value.poseAxisLength > 0.0f) { - settings.setValue("Display/poseAxisLength", value.poseAxisLength); - } - - settings.setValue("PseudoColor/enabled", value.pseudoColorEnabled); - settings.setValue("PseudoColor/defaultPalette", value.pseudoColorDefaultPalette); - settings.setValue("PseudoColor/period", qMax(1.0f, value.pseudoColorPeriod)); - settings.setValue("PseudoColor/startColor", - QColor::fromRgbF(value.pseudoColorStartColor.x(), - value.pseudoColorStartColor.y(), - value.pseudoColorStartColor.z()).name(QColor::HexRgb)); - settings.setValue("PseudoColor/endColor", - QColor::fromRgbF(value.pseudoColorEndColor.x(), - value.pseudoColorEndColor.y(), - value.pseudoColorEndColor.z()).name(QColor::HexRgb)); - - const PointCloudXYZRange& range = value.pointCloudRange; - const bool saveRange = range.enabled && range.isValid(); - settings.setValue("PointCloudRange/enabled", saveRange); - const int filterModeValue = range.filterMode == PointCloudXYZRangeFilterMode::Remove - ? static_cast(PointCloudXYZRangeFilterMode::Remove) - : static_cast(PointCloudXYZRangeFilterMode::ReplaceWithZero); - settings.setValue("PointCloudRange/filterMode", filterModeValue); - settings.setValue("PointCloudRange/mode", filterModeValue); - if (saveRange) { - const auto saveBound = [&settings](const char* key, double bound) { - if (std::isfinite(bound)) { - settings.setValue(key, bound); - } else { - settings.remove(key); - } - }; - saveBound("PointCloudRange/minX", range.minX); - saveBound("PointCloudRange/maxX", range.maxX); - saveBound("PointCloudRange/minY", range.minY); - saveBound("PointCloudRange/maxY", range.maxY); - saveBound("PointCloudRange/minZ", range.minZ); - saveBound("PointCloudRange/maxZ", range.maxZ); - } else { - // Disabled ranges use implicit infinite bounds; do not persist infinities. - settings.remove("PointCloudRange/minX"); - settings.remove("PointCloudRange/maxX"); - settings.remove("PointCloudRange/minY"); - settings.remove("PointCloudRange/maxY"); - settings.remove("PointCloudRange/minZ"); - settings.remove("PointCloudRange/maxZ"); - } - - // Remove keys from the former min/max/stop based implementation so old - // values cannot reappear in the user configuration file. - settings.remove("PseudoColor/autoRange"); - settings.remove("PseudoColor/minZ"); - settings.remove("PseudoColor/maxZ"); - settings.remove("PseudoColor/divisionCount"); - settings.remove("PseudoColor/gradient"); - settings.remove("PseudoColor/stops"); - - settings.sync(); - return settings.status() == QSettings::NoError; -} +#include "CloudViewSettings.h" + +#include +#include +#include +#include +#include + +namespace +{ +QString settingsFilePath() +{ + QString settingsDir = QStandardPaths::writableLocation(QStandardPaths::AppDataLocation); + if (settingsDir.isEmpty()) { + settingsDir = QDir::homePath() + "/CloudView"; + } + QDir().mkpath(settingsDir); + return QDir(settingsDir).filePath("CloudView.ini"); +} +} + +CloudViewSettingsData CloudViewSettings::load() +{ + CloudViewSettingsData result; + QSettings settings(settingsFilePath(), QSettings::IniFormat); + + // 模式独立于启用状态保存；缺失或非法配置默认采用“替换为全 0”。 + const QVariant savedModeValue = settings.contains("PointCloudRange/filterMode") + ? settings.value("PointCloudRange/filterMode") + : settings.value("PointCloudRange/mode", + static_cast(PointCloudXYZRangeFilterMode::ReplaceWithZero)); + const int savedFilterMode = savedModeValue.toInt(); + const PointCloudXYZRangeFilterMode filterMode = + savedFilterMode == static_cast(PointCloudXYZRangeFilterMode::Remove) + ? PointCloudXYZRangeFilterMode::Remove + : PointCloudXYZRangeFilterMode::ReplaceWithZero; + + const float poseAxisLength = settings.value("Display/poseAxisLength", result.poseAxisLength).toFloat(); + if (std::isfinite(poseAxisLength) && poseAxisLength > 0.0f) { + result.poseAxisLength = poseAxisLength; + } + + result.pseudoColorEnabled = settings.value("PseudoColor/enabled", result.pseudoColorEnabled).toBool(); + const bool hasPaletteSetting = settings.contains("PseudoColor/defaultPalette"); + result.pseudoColorDefaultPalette = hasPaletteSetting + ? settings.value("PseudoColor/defaultPalette").toBool() + : result.pseudoColorDefaultPalette; + const float period = settings.value("PseudoColor/period", result.pseudoColorPeriod).toFloat(); + if (std::isfinite(period) && period > 0.0f) { + result.pseudoColorPeriod = period; + } + const QColor startColor(settings.value("PseudoColor/startColor", "#0000ff").toString()); + const QColor endColor(settings.value("PseudoColor/endColor", "#ff0000").toString()); + if (startColor.isValid()) { + result.pseudoColorStartColor = QVector3D(startColor.redF(), startColor.greenF(), startColor.blueF()); + } + if (endColor.isValid()) { + result.pseudoColorEndColor = QVector3D(endColor.redF(), endColor.greenF(), endColor.blueF()); + } + + if (!hasPaletteSetting && settings.contains("PseudoColor/period")) { + const QColor legacyStartColor(settings.value("PseudoColor/startColor", "#0000ff").toString()); + const QColor legacyEndColor(settings.value("PseudoColor/endColor", "#ff0000").toString()); + const bool legacyDefaults = qFuzzyCompare(result.pseudoColorPeriod, 100.0f) && + legacyStartColor == QColor("#0000ff") && + legacyEndColor == QColor("#ff0000"); + if (legacyDefaults) { + // 旧版周期色环配置迁移为 Z 范围蓝→红梯度。 + result.pseudoColorPeriod = 500.0f; + result.pseudoColorStartColor = QVector3D(0.0f, 0.0f, 1.0f); + result.pseudoColorEndColor = QVector3D(1.0f, 0.0f, 0.0f); + result.pseudoColorDefaultPalette = true; + } else { + // 非默认旧配置继续使用原有的自定义起止色渐变。 + result.pseudoColorDefaultPalette = false; + } + } + + const bool rangeEnabled = settings.value("PointCloudRange/enabled", false).toBool(); + PointCloudXYZRange range; + range.enabled = rangeEnabled; + range.filterMode = filterMode; + if (rangeEnabled) { + const auto readBound = [&settings](const char* key, double fallback) { + bool ok = false; + const double value = settings.value(key).toDouble(&ok); + return ok && std::isfinite(value) ? value : fallback; + }; + // Missing bounds retain the range object's implicit unbounded side. + range.minX = readBound("PointCloudRange/minX", range.minX); + range.maxX = readBound("PointCloudRange/maxX", range.maxX); + range.minY = readBound("PointCloudRange/minY", range.minY); + range.maxY = readBound("PointCloudRange/maxY", range.maxY); + range.minZ = readBound("PointCloudRange/minZ", range.minZ); + range.maxZ = readBound("PointCloudRange/maxZ", range.maxZ); + if (range.isValid()) { + result.pointCloudRange = range; + } + } else { + result.pointCloudRange = range; + } + // Keep the mode even when a persisted enabled range is invalid and is + // therefore reset to the disabled default range. + result.pointCloudRange.filterMode = filterMode; + + return result; +} + +bool CloudViewSettings::save(const CloudViewSettingsData& value) +{ + QSettings settings(settingsFilePath(), QSettings::IniFormat); + if (std::isfinite(value.poseAxisLength) && value.poseAxisLength > 0.0f) { + settings.setValue("Display/poseAxisLength", value.poseAxisLength); + } + + settings.setValue("PseudoColor/enabled", value.pseudoColorEnabled); + settings.setValue("PseudoColor/defaultPalette", value.pseudoColorDefaultPalette); + settings.setValue("PseudoColor/period", qMax(1.0f, value.pseudoColorPeriod)); + settings.setValue("PseudoColor/startColor", + QColor::fromRgbF(value.pseudoColorStartColor.x(), + value.pseudoColorStartColor.y(), + value.pseudoColorStartColor.z()).name(QColor::HexRgb)); + settings.setValue("PseudoColor/endColor", + QColor::fromRgbF(value.pseudoColorEndColor.x(), + value.pseudoColorEndColor.y(), + value.pseudoColorEndColor.z()).name(QColor::HexRgb)); + + const PointCloudXYZRange& range = value.pointCloudRange; + const bool saveRange = range.enabled && range.isValid(); + settings.setValue("PointCloudRange/enabled", saveRange); + const int filterModeValue = range.filterMode == PointCloudXYZRangeFilterMode::Remove + ? static_cast(PointCloudXYZRangeFilterMode::Remove) + : static_cast(PointCloudXYZRangeFilterMode::ReplaceWithZero); + settings.setValue("PointCloudRange/filterMode", filterModeValue); + settings.setValue("PointCloudRange/mode", filterModeValue); + if (saveRange) { + const auto saveBound = [&settings](const char* key, double bound) { + if (std::isfinite(bound)) { + settings.setValue(key, bound); + } else { + settings.remove(key); + } + }; + saveBound("PointCloudRange/minX", range.minX); + saveBound("PointCloudRange/maxX", range.maxX); + saveBound("PointCloudRange/minY", range.minY); + saveBound("PointCloudRange/maxY", range.maxY); + saveBound("PointCloudRange/minZ", range.minZ); + saveBound("PointCloudRange/maxZ", range.maxZ); + } else { + // Disabled ranges use implicit infinite bounds; do not persist infinities. + settings.remove("PointCloudRange/minX"); + settings.remove("PointCloudRange/maxX"); + settings.remove("PointCloudRange/minY"); + settings.remove("PointCloudRange/maxY"); + settings.remove("PointCloudRange/minZ"); + settings.remove("PointCloudRange/maxZ"); + } + + // Remove keys from the former min/max/stop based implementation so old + // values cannot reappear in the user configuration file. + settings.remove("PseudoColor/autoRange"); + settings.remove("PseudoColor/minZ"); + settings.remove("PseudoColor/maxZ"); + settings.remove("PseudoColor/divisionCount"); + settings.remove("PseudoColor/gradient"); + settings.remove("PseudoColor/stops"); + + settings.sync(); + return settings.status() == QSettings::NoError; +} diff --git a/CloudViewApp/main.cpp b/CloudViewApp/main.cpp index 5fb3234..0e0f9c4 100644 --- a/CloudViewApp/main.cpp +++ b/CloudViewApp/main.cpp @@ -1,102 +1,102 @@ -#include -#include -#include -#include -#include -#include "CloudViewMainWindow.h" - -#define APP_VERSION "1.2.1" - -int main(int argc, char* argv[]) -{ - QApplication app(argc, argv); - - // 设置 OpenGL 格式 - 使用 2.1 版本以确保固定管线兼容 - QSurfaceFormat format; - format.setDepthBufferSize(24); - format.setStencilBufferSize(8); - format.setVersion(2, 1); // 使用 OpenGL 2.1 以确保固定管线函数可用 - format.setSamples(4); // 抗锯齿 - QSurfaceFormat::setDefaultFormat(format); - - // 设置应用信息 - app.setApplicationName("CloudViewApp"); - app.setOrganizationName("CloudViewApp"); - app.setApplicationVersion(APP_VERSION); - - QCommandLineParser parser; - parser.setApplicationDescription("CloudView point cloud and shape viewer"); - parser.addHelpOption(); - parser.addVersionOption(); - - QCommandLineOption cloudOption( - QStringList() << "cloud" << "point-cloud", - "Load a point cloud or segment file. Positional files are treated the same way.", - "file"); - QCommandLineOption bboxOption( - QStringList() << "bbox", - "Load a bounding-box JSON file.", - "file"); - QCommandLineOption circleOption( - QStringList() << "circle", - "Load a circle data file. Row format: {x,y,z}-{r,p,y}-{radius}.", - "file"); - QCommandLineOption rectangleOption( - QStringList() << "rectangle", - "Load a rectangle data file. Row format: {x,y,z}-{r,p,y}-{width,height}.", - "file"); - QCommandLineOption surfaceOption( - QStringList() << "surface", - "Load a surface data file. Row format: {x,y,z}-{length,width} or {x,y,z}-{r,p,y}-{length,width}.", - "file"); - QCommandLineOption triangleOption( - QStringList() << "triangle", - "Load a triangle data file. Row format: {x,y,z}-{x,y,z}-{x,y,z}.", - "file"); - QCommandLineOption sequenceOption( - QStringList() << "sequence" << "sequential", - "Display startup files one at a time. Closing the window loads the next file."); - - parser.addOption(cloudOption); - parser.addOption(bboxOption); - parser.addOption(circleOption); - parser.addOption(rectangleOption); - parser.addOption(surfaceOption); - parser.addOption(triangleOption); - parser.addOption(sequenceOption); - parser.addPositionalArgument("files", "Point cloud or segment files to load."); - if (app.arguments().contains(QStringLiteral("--help")) || - app.arguments().contains(QStringLiteral("-h"))) { - parser.showHelp(0); - } - if (app.arguments().contains(QStringLiteral("--version")) || - app.arguments().contains(QStringLiteral("-v"))) { - parser.showVersion(); - } - parser.process(app); - - CloudViewMainWindow::StartupFiles startupFiles; - startupFiles.displayFiles = parser.values(cloudOption); - startupFiles.displayFiles.append(parser.positionalArguments()); - startupFiles.bboxFiles = parser.values(bboxOption); - startupFiles.circleFiles = parser.values(circleOption); - startupFiles.rectangleFiles = parser.values(rectangleOption); - startupFiles.surfaceFiles = parser.values(surfaceOption); - startupFiles.triangleFiles = parser.values(triangleOption); - startupFiles.sequential = parser.isSet(sequenceOption); - - // 设置应用程序图标 - app.setWindowIcon(QIcon(":/logo.png")); - - // 创建并显示主窗口 - CloudViewMainWindow mainWindow; - mainWindow.setWindowTitle(QString("点云查看器 v%1").arg(APP_VERSION)); - mainWindow.resize(1280, 720); - mainWindow.show(); - - QTimer::singleShot(0, &mainWindow, [&mainWindow, startupFiles]() { - mainWindow.loadStartupFiles(startupFiles); - }); - - return app.exec(); -} +#include +#include +#include +#include +#include +#include "CloudViewMainWindow.h" + +#define APP_VERSION "1.2.1" + +int main(int argc, char* argv[]) +{ + QApplication app(argc, argv); + + // 设置 OpenGL 格式 - 使用 2.1 版本以确保固定管线兼容 + QSurfaceFormat format; + format.setDepthBufferSize(24); + format.setStencilBufferSize(8); + format.setVersion(2, 1); // 使用 OpenGL 2.1 以确保固定管线函数可用 + format.setSamples(4); // 抗锯齿 + QSurfaceFormat::setDefaultFormat(format); + + // 设置应用信息 + app.setApplicationName("CloudViewApp"); + app.setOrganizationName("CloudViewApp"); + app.setApplicationVersion(APP_VERSION); + + QCommandLineParser parser; + parser.setApplicationDescription("CloudView point cloud and shape viewer"); + parser.addHelpOption(); + parser.addVersionOption(); + + QCommandLineOption cloudOption( + QStringList() << "cloud" << "point-cloud", + "Load a point cloud or segment file. Positional files are treated the same way.", + "file"); + QCommandLineOption bboxOption( + QStringList() << "bbox", + "Load a bounding-box JSON file.", + "file"); + QCommandLineOption circleOption( + QStringList() << "circle", + "Load a circle data file. Row format: {x,y,z}-{r,p,y}-{radius}.", + "file"); + QCommandLineOption rectangleOption( + QStringList() << "rectangle", + "Load a rectangle data file. Row format: {x,y,z}-{r,p,y}-{width,height}.", + "file"); + QCommandLineOption surfaceOption( + QStringList() << "surface", + "Load a surface data file. Row format: {x,y,z}-{length,width} or {x,y,z}-{r,p,y}-{length,width}.", + "file"); + QCommandLineOption triangleOption( + QStringList() << "triangle", + "Load a triangle data file. Row format: {x,y,z}-{x,y,z}-{x,y,z}.", + "file"); + QCommandLineOption sequenceOption( + QStringList() << "sequence" << "sequential", + "Display startup files one at a time. Closing the window loads the next file."); + + parser.addOption(cloudOption); + parser.addOption(bboxOption); + parser.addOption(circleOption); + parser.addOption(rectangleOption); + parser.addOption(surfaceOption); + parser.addOption(triangleOption); + parser.addOption(sequenceOption); + parser.addPositionalArgument("files", "Point cloud or segment files to load."); + if (app.arguments().contains(QStringLiteral("--help")) || + app.arguments().contains(QStringLiteral("-h"))) { + parser.showHelp(0); + } + if (app.arguments().contains(QStringLiteral("--version")) || + app.arguments().contains(QStringLiteral("-v"))) { + parser.showVersion(); + } + parser.process(app); + + CloudViewMainWindow::StartupFiles startupFiles; + startupFiles.displayFiles = parser.values(cloudOption); + startupFiles.displayFiles.append(parser.positionalArguments()); + startupFiles.bboxFiles = parser.values(bboxOption); + startupFiles.circleFiles = parser.values(circleOption); + startupFiles.rectangleFiles = parser.values(rectangleOption); + startupFiles.surfaceFiles = parser.values(surfaceOption); + startupFiles.triangleFiles = parser.values(triangleOption); + startupFiles.sequential = parser.isSet(sequenceOption); + + // 设置应用程序图标 + app.setWindowIcon(QIcon(":/logo.png")); + + // 创建并显示主窗口 + CloudViewMainWindow mainWindow; + mainWindow.setWindowTitle(QString("点云查看器 v%1").arg(APP_VERSION)); + mainWindow.resize(1280, 720); + mainWindow.show(); + + QTimer::singleShot(0, &mainWindow, [&mainWindow, startupFiles]() { + mainWindow.loadStartupFiles(startupFiles); + }); + + return app.exec(); +} diff --git a/ImageView/ImageView.pro b/ImageView/ImageView.pro index 5a0bf22..bd4192a 100644 --- a/ImageView/ImageView.pro +++ b/ImageView/ImageView.pro @@ -1,17 +1,17 @@ -QT += core gui widgets - -TEMPLATE = lib -CONFIG += staticlib c++11 -TARGET = ImageView - -win32-msvc { - QMAKE_CXXFLAGS += /utf-8 -} - -INCLUDEPATH += $$PWD/Inc - -HEADERS += \ - Inc/ImageView.h - -SOURCES += \ - Src/ImageView.cpp +QT += core gui widgets + +TEMPLATE = lib +CONFIG += staticlib c++11 +TARGET = ImageView + +win32-msvc { + QMAKE_CXXFLAGS += /utf-8 +} + +INCLUDEPATH += $$PWD/Inc + +HEADERS += \ + Inc/ImageView.h + +SOURCES += \ + Src/ImageView.cpp diff --git a/ImageView/Inc/ImageView.h b/ImageView/Inc/ImageView.h index a9b81b2..16c83ce 100644 --- a/ImageView/Inc/ImageView.h +++ b/ImageView/Inc/ImageView.h @@ -1,74 +1,74 @@ -#pragma once - -#include -#include -#include -#include - -class QLabel; -class QGraphicsPixmapItem; -class QGraphicsScene; -class QMouseEvent; -class QWheelEvent; - -// 可缩放、可平移的图片显示控件。 -class ImageView : public QGraphicsView -{ - Q_OBJECT - -public: - explicit ImageView(QWidget* parent = nullptr); - ~ImageView() override; - - void setImage(const QImage& image); - void setImage(const QPixmap& pixmap); - bool loadImage(const QString& filePath); - void clearImage(); - - bool hasImage() const; - QImage image() const; - - // 比例以原始图片大小为基准，1.0 表示 100%。 - double zoomRatio() const; - void setZoomRatio(double ratio); - void zoomIn(double factor = 1.25); - void zoomOut(double factor = 1.25); - void resetView(); - - void setZoomRange(double minimum, double maximum); - double minimumZoomRatio() const; - double maximumZoomRatio() const; - - // 图片未铺满视口时的停靠方向，默认居中。 - void setImageAlignment(Qt::Alignment alignment); - Qt::Alignment imageAlignment() const; - -signals: - void imageChanged(); - void zoomRatioChanged(double ratio); - -protected: - void resizeEvent(QResizeEvent* event) override; - void wheelEvent(QWheelEvent* event) override; - void mousePressEvent(QMouseEvent* event) override; - void mouseMoveEvent(QMouseEvent* event) override; - void mouseReleaseEvent(QMouseEvent* event) override; - void mouseDoubleClickEvent(QMouseEvent* event) override; - -private: - void updateZoomLabel(); - void updateZoomRatio(double ratio, bool keepCenter); - void repositionZoomLabel(); - - QGraphicsScene* m_scene; - QGraphicsPixmapItem* m_pixmapItem; - QLabel* m_zoomLabel; - QImage m_image; - double m_zoomRatio; - double m_minimumZoomRatio; - double m_maximumZoomRatio; - Qt::Alignment m_imageAlignment; - bool m_panning; - bool m_needsInitialFit; - QPoint m_lastMousePosition; -}; +#pragma once + +#include +#include +#include +#include + +class QLabel; +class QGraphicsPixmapItem; +class QGraphicsScene; +class QMouseEvent; +class QWheelEvent; + +// 可缩放、可平移的图片显示控件。 +class ImageView : public QGraphicsView +{ + Q_OBJECT + +public: + explicit ImageView(QWidget* parent = nullptr); + ~ImageView() override; + + void setImage(const QImage& image); + void setImage(const QPixmap& pixmap); + bool loadImage(const QString& filePath); + void clearImage(); + + bool hasImage() const; + QImage image() const; + + // 比例以原始图片大小为基准，1.0 表示 100%。 + double zoomRatio() const; + void setZoomRatio(double ratio); + void zoomIn(double factor = 1.25); + void zoomOut(double factor = 1.25); + void resetView(); + + void setZoomRange(double minimum, double maximum); + double minimumZoomRatio() const; + double maximumZoomRatio() const; + + // 图片未铺满视口时的停靠方向，默认居中。 + void setImageAlignment(Qt::Alignment alignment); + Qt::Alignment imageAlignment() const; + +signals: + void imageChanged(); + void zoomRatioChanged(double ratio); + +protected: + void resizeEvent(QResizeEvent* event) override; + void wheelEvent(QWheelEvent* event) override; + void mousePressEvent(QMouseEvent* event) override; + void mouseMoveEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + void mouseDoubleClickEvent(QMouseEvent* event) override; + +private: + void updateZoomLabel(); + void updateZoomRatio(double ratio, bool keepCenter); + void repositionZoomLabel(); + + QGraphicsScene* m_scene; + QGraphicsPixmapItem* m_pixmapItem; + QLabel* m_zoomLabel; + QImage m_image; + double m_zoomRatio; + double m_minimumZoomRatio; + double m_maximumZoomRatio; + Qt::Alignment m_imageAlignment; + bool m_panning; + bool m_needsInitialFit; + QPoint m_lastMousePosition; +}; diff --git a/ImageView/Src/ImageView.cpp b/ImageView/Src/ImageView.cpp index 4d53245..2c5f951 100644 --- a/ImageView/Src/ImageView.cpp +++ b/ImageView/Src/ImageView.cpp @@ -1,315 +1,315 @@ -#include "ImageView.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace { -constexpr double kDefaultMinimumZoom = 0.02; -constexpr double kDefaultMaximumZoom = 20.0; -constexpr double kZoomStep = 1.25; -} - -ImageView::ImageView(QWidget* parent) - : QGraphicsView(parent), - m_scene(new QGraphicsScene(this)), - m_pixmapItem(nullptr), - m_zoomLabel(new QLabel(this)), - m_zoomRatio(1.0), - m_minimumZoomRatio(kDefaultMinimumZoom), - m_maximumZoomRatio(kDefaultMaximumZoom), - m_imageAlignment(Qt::AlignCenter), - m_panning(false), - m_needsInitialFit(false) -{ - setScene(m_scene); - setBackgroundBrush(QColor(35, 35, 35)); - setFrameShape(QFrame::NoFrame); - setHorizontalScrollBarPolicy(Qt::ScrollBarAsNeeded); - setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); - setTransformationAnchor(QGraphicsView::AnchorUnderMouse); - setResizeAnchor(QGraphicsView::AnchorViewCenter); - setAlignment(m_imageAlignment); - setDragMode(QGraphicsView::NoDrag); - setCursor(Qt::OpenHandCursor); - - m_zoomLabel->setObjectName(QStringLiteral("imageViewZoomLabel")); - m_zoomLabel->setAlignment(Qt::AlignCenter); - m_zoomLabel->setMinimumWidth(58); - m_zoomLabel->setAttribute(Qt::WA_TransparentForMouseEvents); - m_zoomLabel->setStyleSheet(QStringLiteral( - "QLabel { color: white; background: transparent; border: none; }")); - updateZoomLabel(); -} - -ImageView::~ImageView() = default; - -void ImageView::setImage(const QImage& image) -{ - if (image.isNull()) { - clearImage(); - return; - } - - m_image = image; - if (!m_pixmapItem) { - m_pixmapItem = m_scene->addPixmap(QPixmap::fromImage(m_image)); - } else { - m_pixmapItem->setPixmap(QPixmap::fromImage(m_image)); - } - m_scene->setSceneRect(QRectF(QPointF(0.0, 0.0), QSizeF(m_image.size()))); - m_needsInitialFit = true; - if (viewport()->width() > 0 && viewport()->height() > 0) { - m_needsInitialFit = false; - resetView(); - } - emit imageChanged(); -} - -void ImageView::setImage(const QPixmap& pixmap) -{ - setImage(pixmap.toImage()); -} - -bool ImageView::loadImage(const QString& filePath) -{ - QImage loaded; - if (!loaded.load(filePath)) { - return false; - } - setImage(loaded); - return true; -} - -void ImageView::clearImage() -{ - m_image = QImage(); - m_scene->clear(); - m_pixmapItem = nullptr; - m_scene->setSceneRect(QRectF()); - m_zoomRatio = 1.0; - m_needsInitialFit = false; - updateZoomLabel(); - viewport()->update(); - emit imageChanged(); -} - -bool ImageView::hasImage() const -{ - return !m_image.isNull(); -} - -QImage ImageView::image() const -{ - return m_image; -} - -double ImageView::zoomRatio() const -{ - return m_zoomRatio; -} - -void ImageView::setZoomRatio(double ratio) -{ - updateZoomRatio(ratio, true); -} - -void ImageView::zoomIn(double factor) -{ - if (factor <= 1.0) { - factor = kZoomStep; - } - updateZoomRatio(m_zoomRatio * factor, true); -} - -void ImageView::zoomOut(double factor) -{ - if (factor <= 1.0) { - factor = kZoomStep; - } - updateZoomRatio(m_zoomRatio / factor, true); -} - -void ImageView::resetView() -{ - if (!hasImage() || !m_pixmapItem) { - return; - } - - const double previousRatio = m_zoomRatio; - resetTransform(); - fitInView(m_pixmapItem, Qt::KeepAspectRatio); - - const double fittedRatio = qBound(m_minimumZoomRatio, - transform().m11(), - m_maximumZoomRatio); - resetTransform(); - scale(fittedRatio, fittedRatio); - m_zoomRatio = fittedRatio; - centerOn(m_pixmapItem); - updateZoomLabel(); - - if (!qFuzzyCompare(previousRatio, m_zoomRatio)) { - emit zoomRatioChanged(m_zoomRatio); - } -} - -void ImageView::setZoomRange(double minimum, double maximum) -{ - if (minimum <= 0.0 || maximum < minimum) { - return; - } - m_minimumZoomRatio = minimum; - m_maximumZoomRatio = maximum; - updateZoomRatio(m_zoomRatio, true); -} - -double ImageView::minimumZoomRatio() const -{ - return m_minimumZoomRatio; -} - -double ImageView::maximumZoomRatio() const -{ - return m_maximumZoomRatio; -} - -void ImageView::setImageAlignment(Qt::Alignment alignment) -{ - if (!alignment) { - alignment = Qt::AlignCenter; - } - m_imageAlignment = alignment; - setAlignment(m_imageAlignment); - if (hasImage()) { - viewport()->update(); - } -} - -Qt::Alignment ImageView::imageAlignment() const -{ - return m_imageAlignment; -} - -void ImageView::resizeEvent(QResizeEvent* event) -{ - QGraphicsView::resizeEvent(event); - if (m_needsInitialFit && hasImage() && viewport()->width() > 0 && viewport()->height() > 0) { - m_needsInitialFit = false; - resetView(); - } - repositionZoomLabel(); -} - -void ImageView::wheelEvent(QWheelEvent* event) -{ - if (!hasImage() || event->angleDelta().y() == 0) { - event->ignore(); - return; - } - - const double factor = event->angleDelta().y() > 0 ? kZoomStep : 1.0 / kZoomStep; - updateZoomRatio(m_zoomRatio * factor, true); - event->accept(); -} - -void ImageView::mousePressEvent(QMouseEvent* event) -{ - if (event->button() == Qt::LeftButton && hasImage()) { - m_panning = true; - m_lastMousePosition = event->pos(); - setCursor(Qt::ClosedHandCursor); - event->accept(); - return; - } - QGraphicsView::mousePressEvent(event); -} - -void ImageView::mouseMoveEvent(QMouseEvent* event) -{ - if (m_panning) { - const QPoint delta = event->pos() - m_lastMousePosition; - m_lastMousePosition = event->pos(); - horizontalScrollBar()->setValue(horizontalScrollBar()->value() - delta.x()); - verticalScrollBar()->setValue(verticalScrollBar()->value() - delta.y()); - event->accept(); - return; - } - QGraphicsView::mouseMoveEvent(event); -} - -void ImageView::mouseReleaseEvent(QMouseEvent* event) -{ - if (event->button() == Qt::LeftButton && m_panning) { - m_panning = false; - setCursor(Qt::OpenHandCursor); - event->accept(); - return; - } - QGraphicsView::mouseReleaseEvent(event); -} - -void ImageView::mouseDoubleClickEvent(QMouseEvent* event) -{ - if (event->button() == Qt::LeftButton && hasImage()) { - resetView(); - event->accept(); - return; - } - QGraphicsView::mouseDoubleClickEvent(event); -} - -void ImageView::updateZoomLabel() -{ - if (!hasImage()) { - m_zoomLabel->setText(QStringLiteral("--")); - } else { - m_zoomLabel->setText(QStringLiteral("%1%") - .arg(m_zoomRatio * 100.0, 0, 'f', 1)); - } - m_zoomLabel->adjustSize(); - repositionZoomLabel(); -} - -void ImageView::updateZoomRatio(double ratio, bool keepCenter) -{ - if (!hasImage()) { - return; - } - - ratio = qBound(m_minimumZoomRatio, ratio, m_maximumZoomRatio); - if (qFuzzyCompare(ratio, m_zoomRatio) && !transform().isIdentity()) { - return; - } - - const QPoint viewportCenter = viewport()->rect().center(); - const QPointF sceneCenter = mapToScene(viewportCenter); - resetTransform(); - scale(ratio, ratio); - m_zoomRatio = ratio; - if (keepCenter) { - centerOn(sceneCenter); - } - updateZoomLabel(); - emit zoomRatioChanged(m_zoomRatio); -} - -void ImageView::repositionZoomLabel() -{ - if (!m_zoomLabel) { - return; - } - const int margin = 8; - const int scrollBarHeight = horizontalScrollBar()->isVisible() - ? horizontalScrollBar()->height() - : 0; - m_zoomLabel->move(margin, - height() - m_zoomLabel->height() - margin - scrollBarHeight); - m_zoomLabel->raise(); -} +#include "ImageView.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { +constexpr double kDefaultMinimumZoom = 0.02; +constexpr double kDefaultMaximumZoom = 20.0; +constexpr double kZoomStep = 1.25; +} + +ImageView::ImageView(QWidget* parent) + : QGraphicsView(parent), + m_scene(new QGraphicsScene(this)), + m_pixmapItem(nullptr), + m_zoomLabel(new QLabel(this)), + m_zoomRatio(1.0), + m_minimumZoomRatio(kDefaultMinimumZoom), + m_maximumZoomRatio(kDefaultMaximumZoom), + m_imageAlignment(Qt::AlignCenter), + m_panning(false), + m_needsInitialFit(false) +{ + setScene(m_scene); + setBackgroundBrush(QColor(35, 35, 35)); + setFrameShape(QFrame::NoFrame); + setHorizontalScrollBarPolicy(Qt::ScrollBarAsNeeded); + setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + setTransformationAnchor(QGraphicsView::AnchorUnderMouse); + setResizeAnchor(QGraphicsView::AnchorViewCenter); + setAlignment(m_imageAlignment); + setDragMode(QGraphicsView::NoDrag); + setCursor(Qt::OpenHandCursor); + + m_zoomLabel->setObjectName(QStringLiteral("imageViewZoomLabel")); + m_zoomLabel->setAlignment(Qt::AlignCenter); + m_zoomLabel->setMinimumWidth(58); + m_zoomLabel->setAttribute(Qt::WA_TransparentForMouseEvents); + m_zoomLabel->setStyleSheet(QStringLiteral( + "QLabel { color: white; background: transparent; border: none; }")); + updateZoomLabel(); +} + +ImageView::~ImageView() = default; + +void ImageView::setImage(const QImage& image) +{ + if (image.isNull()) { + clearImage(); + return; + } + + m_image = image; + if (!m_pixmapItem) { + m_pixmapItem = m_scene->addPixmap(QPixmap::fromImage(m_image)); + } else { + m_pixmapItem->setPixmap(QPixmap::fromImage(m_image)); + } + m_scene->setSceneRect(QRectF(QPointF(0.0, 0.0), QSizeF(m_image.size()))); + m_needsInitialFit = true; + if (viewport()->width() > 0 && viewport()->height() > 0) { + m_needsInitialFit = false; + resetView(); + } + emit imageChanged(); +} + +void ImageView::setImage(const QPixmap& pixmap) +{ + setImage(pixmap.toImage()); +} + +bool ImageView::loadImage(const QString& filePath) +{ + QImage loaded; + if (!loaded.load(filePath)) { + return false; + } + setImage(loaded); + return true; +} + +void ImageView::clearImage() +{ + m_image = QImage(); + m_scene->clear(); + m_pixmapItem = nullptr; + m_scene->setSceneRect(QRectF()); + m_zoomRatio = 1.0; + m_needsInitialFit = false; + updateZoomLabel(); + viewport()->update(); + emit imageChanged(); +} + +bool ImageView::hasImage() const +{ + return !m_image.isNull(); +} + +QImage ImageView::image() const +{ + return m_image; +} + +double ImageView::zoomRatio() const +{ + return m_zoomRatio; +} + +void ImageView::setZoomRatio(double ratio) +{ + updateZoomRatio(ratio, true); +} + +void ImageView::zoomIn(double factor) +{ + if (factor <= 1.0) { + factor = kZoomStep; + } + updateZoomRatio(m_zoomRatio * factor, true); +} + +void ImageView::zoomOut(double factor) +{ + if (factor <= 1.0) { + factor = kZoomStep; + } + updateZoomRatio(m_zoomRatio / factor, true); +} + +void ImageView::resetView() +{ + if (!hasImage() || !m_pixmapItem) { + return; + } + + const double previousRatio = m_zoomRatio; + resetTransform(); + fitInView(m_pixmapItem, Qt::KeepAspectRatio); + + const double fittedRatio = qBound(m_minimumZoomRatio, + transform().m11(), + m_maximumZoomRatio); + resetTransform(); + scale(fittedRatio, fittedRatio); + m_zoomRatio = fittedRatio; + centerOn(m_pixmapItem); + updateZoomLabel(); + + if (!qFuzzyCompare(previousRatio, m_zoomRatio)) { + emit zoomRatioChanged(m_zoomRatio); + } +} + +void ImageView::setZoomRange(double minimum, double maximum) +{ + if (minimum <= 0.0 || maximum < minimum) { + return; + } + m_minimumZoomRatio = minimum; + m_maximumZoomRatio = maximum; + updateZoomRatio(m_zoomRatio, true); +} + +double ImageView::minimumZoomRatio() const +{ + return m_minimumZoomRatio; +} + +double ImageView::maximumZoomRatio() const +{ + return m_maximumZoomRatio; +} + +void ImageView::setImageAlignment(Qt::Alignment alignment) +{ + if (!alignment) { + alignment = Qt::AlignCenter; + } + m_imageAlignment = alignment; + setAlignment(m_imageAlignment); + if (hasImage()) { + viewport()->update(); + } +} + +Qt::Alignment ImageView::imageAlignment() const +{ + return m_imageAlignment; +} + +void ImageView::resizeEvent(QResizeEvent* event) +{ + QGraphicsView::resizeEvent(event); + if (m_needsInitialFit && hasImage() && viewport()->width() > 0 && viewport()->height() > 0) { + m_needsInitialFit = false; + resetView(); + } + repositionZoomLabel(); +} + +void ImageView::wheelEvent(QWheelEvent* event) +{ + if (!hasImage() || event->angleDelta().y() == 0) { + event->ignore(); + return; + } + + const double factor = event->angleDelta().y() > 0 ? kZoomStep : 1.0 / kZoomStep; + updateZoomRatio(m_zoomRatio * factor, true); + event->accept(); +} + +void ImageView::mousePressEvent(QMouseEvent* event) +{ + if (event->button() == Qt::LeftButton && hasImage()) { + m_panning = true; + m_lastMousePosition = event->pos(); + setCursor(Qt::ClosedHandCursor); + event->accept(); + return; + } + QGraphicsView::mousePressEvent(event); +} + +void ImageView::mouseMoveEvent(QMouseEvent* event) +{ + if (m_panning) { + const QPoint delta = event->pos() - m_lastMousePosition; + m_lastMousePosition = event->pos(); + horizontalScrollBar()->setValue(horizontalScrollBar()->value() - delta.x()); + verticalScrollBar()->setValue(verticalScrollBar()->value() - delta.y()); + event->accept(); + return; + } + QGraphicsView::mouseMoveEvent(event); +} + +void ImageView::mouseReleaseEvent(QMouseEvent* event) +{ + if (event->button() == Qt::LeftButton && m_panning) { + m_panning = false; + setCursor(Qt::OpenHandCursor); + event->accept(); + return; + } + QGraphicsView::mouseReleaseEvent(event); +} + +void ImageView::mouseDoubleClickEvent(QMouseEvent* event) +{ + if (event->button() == Qt::LeftButton && hasImage()) { + resetView(); + event->accept(); + return; + } + QGraphicsView::mouseDoubleClickEvent(event); +} + +void ImageView::updateZoomLabel() +{ + if (!hasImage()) { + m_zoomLabel->setText(QStringLiteral("--")); + } else { + m_zoomLabel->setText(QStringLiteral("%1%") + .arg(m_zoomRatio * 100.0, 0, 'f', 1)); + } + m_zoomLabel->adjustSize(); + repositionZoomLabel(); +} + +void ImageView::updateZoomRatio(double ratio, bool keepCenter) +{ + if (!hasImage()) { + return; + } + + ratio = qBound(m_minimumZoomRatio, ratio, m_maximumZoomRatio); + if (qFuzzyCompare(ratio, m_zoomRatio) && !transform().isIdentity()) { + return; + } + + const QPoint viewportCenter = viewport()->rect().center(); + const QPointF sceneCenter = mapToScene(viewportCenter); + resetTransform(); + scale(ratio, ratio); + m_zoomRatio = ratio; + if (keepCenter) { + centerOn(sceneCenter); + } + updateZoomLabel(); + emit zoomRatioChanged(m_zoomRatio); +} + +void ImageView::repositionZoomLabel() +{ + if (!m_zoomLabel) { + return; + } + const int margin = 8; + const int scrollBarHeight = horizontalScrollBar()->isVisible() + ? horizontalScrollBar()->height() + : 0; + m_zoomLabel->move(margin, + height() - m_zoomLabel->height() - margin - scrollBarHeight); + m_zoomLabel->raise(); +}