From 176a833bb27ae1e356487c4f3907b46743762fef Mon Sep 17 00:00:00 2001 From: snso Date: Wed, 5 Aug 2026 11:48:29 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AF=B9=E7=95=8C=E9=9D=A2=E5=81=9A=E4=BA=86?= =?UTF-8?q?=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 6 +- CloudUtils/CloudUtils.pro | 15 +- CloudUtils/Src/LaserDataLoader.cpp | 1536 +++++++++++++----------- CloudView/CloudView.pro | 25 +- CloudView/Src/CloudViewMainWindow.cpp | 42 +- CloudView/main.cpp | 8 + CloudView3D/CloudView3D.pro | 9 +- CloudView3D/Inc/PointCloudGLWidget.h | 13 +- CloudView3D/Src/PointCloudGLWidget.cpp | 251 +++- VrCommon/VrCommon.pro | 17 +- VrUtils/VrUtils.pro | 13 +- 11 files changed, 1125 insertions(+), 810 deletions(-) diff --git a/.gitignore b/.gitignore index 51db085..d526a0c 100644 --- a/.gitignore +++ b/.gitignore @@ -15,8 +15,10 @@ Makefile* moc_*.cpp moc_*.h -qrc_*.cpp -ui_*.h +qrc_*.cpp +*_resource.rc +*_resource.res +ui_*.h *.autosave # 构建目录 diff --git a/CloudUtils/CloudUtils.pro b/CloudUtils/CloudUtils.pro index d62a055..70fb86f 100644 --- a/CloudUtils/CloudUtils.pro +++ b/CloudUtils/CloudUtils.pro @@ -4,10 +4,17 @@ QT += core # 鎸囧畾椤圭洰妯℃澘涓洪潤鎬佸簱 -TEMPLATE = lib -CONFIG += staticlib - -win32-msvc { +TEMPLATE = lib +CONFIG += staticlib + +win32:CONFIG(debug, debug|release) { + DESTDIR = $$PWD/debug +} +win32:CONFIG(release, debug|release) { + DESTDIR = $$PWD/release +} + +win32-msvc { QMAKE_CXXFLAGS += /utf-8 } diff --git a/CloudUtils/Src/LaserDataLoader.cpp b/CloudUtils/Src/LaserDataLoader.cpp index 99bc738..d7537e2 100644 --- a/CloudUtils/Src/LaserDataLoader.cpp +++ b/CloudUtils/Src/LaserDataLoader.cpp @@ -14,10 +14,10 @@ #include #include #include - -#include "VrLog.h" - -// 辅助函数：去除字符串末尾的 \r 字符（处理 Windows 格式的 CR LF 换行符） + +#include "VrLog.h" + +// 辅助函数：去除字符串末尾的 \r 字符（处理 Windows 格式的 CR LF 换行符） static inline void TrimCarriageReturn(std::string& str) { if (!str.empty() && str.back() == '\r') { @@ -158,153 +158,204 @@ void AppendFormattedLine(std::string& output, const char* format, ...) output.append(dynamicBuffer.data(), static_cast(written)); } } - -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) -{ - LOG_INFO("Loading from file: %s\n", fileName.c_str()); - - // 清空输出参数 - laserLines.clear(); - lineNum = 0; - scanSpeed = 0.0f; - maxTimeStamp = 0; + +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) +{ + LOG_INFO("Loading from file: %s\n", fileName.c_str()); + + // 清空输出参数 + laserLines.clear(); + lineNum = 0; + scanSpeed = 0.0f; + maxTimeStamp = 0; clockPerSecond = 0; if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) { return _LoadLaserScanDataFromDatFile(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond); } - - // 判断文件类型 - std::ifstream inputFile(fileName); - if (!inputFile.is_open()) { - m_lastError = "Cannot open file: " + fileName; - LOG_ERROR("Cannot open file: %s\n", fileName.c_str()); - return ERR_CODE(FILE_ERR_NOEXIST); - } - - std::string line; - int result = SUCCESS; - - // 包含DataType字段，检查数据类型 - EVzResultDataType eDataType = keResultDataType_Invalid; - result = _GetLaserType(fileName, eDataType); - ERR_CODE_RETURN(result); - - LOG_INFO("Laser data type: %d \n", eDataType); - - SVzLaserLineData sLaserData; - memset(&sLaserData, 0, sizeof(SVzLaserLineData)); - - bool bFindLineNum = true; - int nLaserPointIdx = 0; - - while (std::getline(inputFile, line)) { - // 去除行末的 \r 字符（处理 Windows 格式的 CR LF 换行符） - TrimCarriageReturn(line); - - if (line.find("LineNum:") == 0) { - sscanf(line.c_str(), "LineNum:%d", &lineNum); - } else if (line.find("DataType:") == 0) { - - } else if (line.find("Line_") == 0) { - - if(false == bFindLineNum) { - laserLines.push_back(std::make_pair(eDataType, sLaserData)); - } - - int lineIndex; - unsigned int timeStamp; - int ptNum = 0; - sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); - - sLaserData.llFrameIdx = lineIndex; - sLaserData.llTimeStamp = timeStamp; - sLaserData.nPointCount = ptNum; - if (eDataType == keResultDataType_PointXYZRGBA) { - sLaserData.p3DPoint = new SVzNLPointXYZRGBA[ptNum]; - sLaserData.p2DPoint = new SVzNL2DLRPoint[ptNum]; - memset(sLaserData.p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * ptNum); - memset(sLaserData.p2DPoint, 0, sizeof(SVzNL2DLRPoint) * ptNum); - } else if(eDataType == keResultDataType_Position) { - sLaserData.p3DPoint = new SVzNL3DPosition[ptNum]; - sLaserData.p2DPoint = new SVzNL2DPosition[ptNum]; - memset(sLaserData.p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); - memset(sLaserData.p2DPoint, 0, sizeof(SVzNL2DPosition) * ptNum); - } - nLaserPointIdx = 0; - bFindLineNum = false; - - } else if (line.find("Poly_") == 0) { - // 跳过 Poly 折线头及后续点坐标数据 - size_t lastUnderscore = line.rfind('_'); - int polyNum = 0; - if (lastUnderscore != std::string::npos && lastUnderscore + 1 < line.size()) { - polyNum = std::atoi(line.c_str() + lastUnderscore + 1); - } - for (int skip = 0; skip < polyNum; ) { - if (!std::getline(inputFile, line)) break; - TrimCarriageReturn(line); - if (!line.empty()) { - ++skip; - } - } - - } else if (line.find("{") == 0) { - // 使用正则表达式判断是XYZ还是RGBD格式 - // XYZ格式: {x,y,z}-{leftX,leftY}-{rightX,rightY} - // RGBD格式: {x,y,z,r,g,b}-{leftX,leftY}-{rightX,rightY} - - // 更精确的正则表达式，匹配完整的行格式 - if(sLaserData.p3DPoint == nullptr || sLaserData.p2DPoint == nullptr) { - LOG_ERROR("sLaserData.p3DPoint == nullptr || sLaserData.p2DPoint == nullptr \n"); - return ERR_CODE(DATA_ERR_INVALID); - } - - // 防止数组越界：跳过超出声明点数的数据行 - if (nLaserPointIdx >= sLaserData.nPointCount) { - LOG_WARN("nLaserPointIdx(%d) >= nPointCount(%d), skip\n", nLaserPointIdx, sLaserData.nPointCount); - continue; - } - - if (eDataType == keResultDataType_PointXYZRGBA) { - SVzNLPointXYZRGBA* pRGBAPoints = static_cast(sLaserData.p3DPoint); - SVzNL2DLRPoint* p2DPoints = static_cast(sLaserData.p2DPoint); - _ParseLaserScanPoint(line, pRGBAPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]); - nLaserPointIdx++; - } else { - SVzNL3DPosition* p3DPoints = static_cast(sLaserData.p3DPoint); - SVzNL2DPosition* p2DPoints = static_cast(sLaserData.p2DPoint); - _ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]); - p3DPoints[nLaserPointIdx].nPointIdx = nLaserPointIdx; - nLaserPointIdx++; - } - } - } - - // 添加最后一条扫描线数据 - if (!bFindLineNum) { - laserLines.push_back(std::make_pair(eDataType, sLaserData)); - } - - inputFile.close(); - LOG_INFO("Successfully loaded %d laser scan lines from file: %s\n", lineNum, fileName.c_str()); - return SUCCESS; -} - + + // 判断文件类型 + 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()); + // } + 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) { + 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 (line.find("Poly_") == 0) { + // 跳过 Poly 折线头及后续点坐标数据 + size_t lastUnderscore = line.rfind('_'); + int polyNum = 0; + if (lastUnderscore != std::string::npos && lastUnderscore + 1 < line.size()) { + polyNum = std::atoi(line.c_str() + lastUnderscore + 1); + } + for (int skip = 0; skip < polyNum; ) { + if (!std::getline(inputFile, line)) { + return failTextLoad(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT, + "Incomplete Poly 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 ERR_CODE(DATA_ERR_INVALID); + } + + // 防止数组越界：跳过超出声明点数的数据行 + if (nLaserPointIdx >= sLaserData.nPointCount) { + LOG_WARN("nLaserPointIdx(%d) >= nPointCount(%d), skip\n", nLaserPointIdx, sLaserData.nPointCount); + continue; + } + + if (eDataType == keResultDataType_PointXYZRGBA) { + SVzNLPointXYZRGBA* pRGBAPoints = static_cast(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 (inputFile.bad()) { + return failTextLoad(FILE_ERR_READ, "Failed while reading laser file: " + fileName); + } + + // 添加最后一条扫描线数据 + if (!bFindLineNum) { + result = commitCurrentLine(); + if (SUCCESS != result) return result; + } + if (lineNum <= 0 || laserLines.size() != static_cast(lineNum)) { + std::ostringstream message; + message << "Laser line count mismatch: expected " << lineNum + << ", got " << laserLines.size(); + return failTextLoad(FILE_ERR_FORMAT, message.str()); + } + + inputFile.close(); + LOG_INFO("Successfully loaded %d laser scan lines from file: %s\n", lineNum, fileName.c_str()); + return SUCCESS; +} + // Read binary .dat laser scan data. int LaserDataLoader::_LoadLaserScanDataFromDatFile(const std::string& fileName, std::vector>& laserLines, @@ -474,44 +525,57 @@ 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); - } - - // 写入文件头 + 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); + } + const EVzResultDataType fileDataType = laserLines.front().first; + if (fileDataType != keResultDataType_Position && + fileDataType != keResultDataType_PointXYZI && + fileDataType != keResultDataType_PointXYZRGBA) { + m_lastError = "Unsupported laser data type for saving"; + return ERR_CODE(DEV_ARG_INVAILD); + } + if (lineNum != static_cast(laserLines.size()) || + std::any_of(laserLines.begin(), laserLines.end(), + [fileDataType](const auto& line) { return line.first != fileDataType; })) { + m_lastError = "Line count mismatch or mixed laser data types"; + 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 << "DataType:" << static_cast(fileDataType) << '\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; - + + // 写入每条扫描线数据 + 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); + + // 根据数据类型写入点云数据 + 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; @@ -523,35 +587,38 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName, float x = static_cast(points[i].pt3D.x); float y = static_cast(points[i].pt3D.y); float z = static_cast(points[i].pt3D.z); + const int leftX = points2D ? points2D[i].ptLeft2D.x : 0; + const int leftY = points2D ? points2D[i].ptLeft2D.y : 0; + const int rightX = points2D ? points2D[i].ptRight2D.x : 0; + const int rightY = points2D ? points2D[i].ptRight2D.y : 0; 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); + leftX, leftY, rightX, rightY); } 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); + } 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; @@ -566,72 +633,81 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName, int r = (points[i].nRGB >> 16) & 0xFF; int g = (points[i].nRGB >> 8) & 0xFF; int b = points[i].nRGB & 0xFF; + const int leftX = points2D ? points2D[i].sLeft.x : 0; + const int leftY = points2D ? points2D[i].sLeft.y : 0; + const int rightX = points2D ? points2D[i].sRight.x : 0; + const int rightY = points2D ? points2D[i].sRight.y : 0; 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); + leftX, leftY, rightX, rightY); } sw.write(buffer.data(), static_cast(buffer.size())); } - } - } - - sw.close(); - return SUCCESS; - - } catch (const std::exception& e) { - m_lastError = "Error saving unified file: " + std::string(e.what()); - LOG_ERROR("Error saving unified laser data to file: %s\n", e.what()); - return ERR_CODE(FILE_ERR_WRITE); - } -} - -int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector > 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.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); - } - + + 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) { @@ -640,8 +716,8 @@ int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector(point.pt3D.x); float y = static_cast(point.pt3D.y); float z = static_cast(point.pt3D.z); @@ -650,170 +726,191 @@ int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector(buffer.size())); } - - ++index; - if (callback) { - callback->onSaveProgress(static_cast(index) / static_cast(totalLines)); - } - } - - sw.close(); - return SUCCESS; - - } catch (const std::exception& e) { - m_lastError = "Error saving unified file: " + std::string(e.what()); - LOG_ERROR("Error saving unified laser data to file: %s\n", e.what()); - return ERR_CODE(FILE_ERR_WRITE); - } -} - -int LaserDataLoader::LoadPolySegments(const std::string& fileName, - std::vector>& polyLines) -{ - LOG_INFO("Loading poly segments from file: %s\n", fileName.c_str()); - - polyLines.clear(); - - std::ifstream inputFile(fileName); - if (!inputFile.is_open()) { - m_lastError = "Cannot open file: " + fileName; - LOG_ERROR("Cannot open file: %s\n", fileName.c_str()); - return ERR_CODE(FILE_ERR_NOEXIST); - } - - std::string line; - - while (std::getline(inputFile, line)) { - TrimCarriageReturn(line); - - // 跳过空行和注释 - if (line.empty() || line[0] == '#') { - continue; - } - - // 查找 Poly_index_num 头 - if (line.find("Poly_") != 0) { - continue; - } - - // 解析点数量（最后一个下划线后的数字） - size_t lastUnderscore = line.rfind('_'); - if (lastUnderscore == std::string::npos || lastUnderscore + 1 >= line.size()) { - LOG_WARN("[LaserDataLoader] Invalid Poly header: %s\n", line.c_str()); - continue; - } - - int num = std::atoi(line.c_str() + lastUnderscore + 1); - if (num < 2) { - LOG_WARN("[LaserDataLoader] Invalid point count in Poly header: %s\n", line.c_str()); - continue; - } - - // 读取 num 个点坐标 - std::vector points; - for (int i = 0; i < num; ) { - if (!std::getline(inputFile, line)) break; - TrimCarriageReturn(line); - if (line.empty()) { - continue; // 跳过空行，不计数 - } - - SVzNLPointXYZRGBA pt; - memset(&pt, 0, sizeof(pt)); - pt.nRGB = 0x00FFFFFF; // 默认白色 - bool parsed = false; - - // 尝试花括号格式 - if (line.find('{') != std::string::npos) { - float fx, fy, fz; - int R = 255, G = 255, B = 255, A = 255; - - // 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A} - int cnt = sscanf(line.c_str(), - " { %f , %f , %f } - { %*f , %*f } - { %*f , %*f } - { %d , %d , %d , %d }", - &fx, &fy, &fz, &R, &G, &B, &A); - if (cnt >= 3) { - pt.x = fx; - pt.y = fy; - pt.z = fz; - if (cnt >= 7) { - // 解析到了颜色 R,G,B,A（0-255），Alpha 存储在高 8 位 - unsigned int nRGB = static_cast(A) << 24; - nRGB |= static_cast(B) << 16; - nRGB |= static_cast(G) << 8; - nRGB |= static_cast(R); - pt.nRGB = nRGB; - } - parsed = true; - } - - // 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry} - if (!parsed) { - float r, g, b; - if (sscanf(line.c_str(), " { %f , %f , %f , %f , %f , %f }", - &fx, &fy, &fz, &r, &g, &b) == 6) { - pt.x = fx; - pt.y = fy; - pt.z = fz; - int nRGB = (int)(b * 255); nRGB <<= 8; - nRGB += (int)(g * 255); nRGB <<= 8; - nRGB += (int)(r * 255); - pt.nRGB = nRGB; - parsed = true; - } - } - - // XYZ格式: {x,y,z}-{...}-{...} - if (!parsed) { - if (sscanf(line.c_str(), " { %f , %f , %f }", &fx, &fy, &fz) == 3) { - pt.x = fx; - pt.y = fy; - pt.z = fz; - parsed = true; - } - } - } - - // 尝试纯数字格式: x y z（空格/Tab/逗号分隔） - if (!parsed) { - float fx, fy, fz; - std::string lineCopy = line; - for (char& c : lineCopy) { - if (c == ',' || c == '\t') c = ' '; - } - if (sscanf(lineCopy.c_str(), "%f %f %f", &fx, &fy, &fz) == 3) { - pt.x = fx; - pt.y = fy; - pt.z = fz; - parsed = true; - } - } - - if (parsed) { - points.push_back(pt); - } - ++i; - } - - if (points.size() >= 2) { - polyLines.push_back(std::move(points)); - } - } - - inputFile.close(); - LOG_INFO("Loaded %zu poly lines from file: %s\n", polyLines.size(), fileName.c_str()); - return SUCCESS; -} - -void LaserDataLoader::FreeLaserScanData(std::vector>& 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; - + + ++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::LoadPolySegments(const std::string& fileName, + std::vector>& polyLines) +{ + LOG_INFO("Loading poly segments from file: %s\n", fileName.c_str()); + + polyLines.clear(); + + std::ifstream inputFile(fileName); + if (!inputFile.is_open()) { + m_lastError = "Cannot open file: " + fileName; + LOG_ERROR("Cannot open file: %s\n", fileName.c_str()); + return ERR_CODE(FILE_ERR_NOEXIST); + } + + std::string line; + + while (std::getline(inputFile, line)) { + TrimCarriageReturn(line); + + // 跳过空行和注释 + if (line.empty() || line[0] == '#') { + continue; + } + + // 查找 Poly_index_num 头 + if (line.find("Poly_") != 0) { + continue; + } + + // 解析点数量（最后一个下划线后的数字） + size_t lastUnderscore = line.rfind('_'); + if (lastUnderscore == std::string::npos || lastUnderscore + 1 >= line.size()) { + LOG_WARN("[LaserDataLoader] Invalid Poly header: %s\n", line.c_str()); + continue; + } + + int num = std::atoi(line.c_str() + lastUnderscore + 1); + if (num < 2) { + LOG_WARN("[LaserDataLoader] Invalid point count in Poly header: %s\n", line.c_str()); + continue; + } + + // 读取 num 个点坐标 + std::vector 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; + } + + // 旧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 Poly block in file: " + fileName; + LOG_ERROR("%s\n", m_lastError.c_str()); + polyLines.clear(); + return ERR_CODE(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT); + } + polyLines.push_back(std::move(points)); + } + + if (inputFile.bad()) { + m_lastError = "Failed while reading Poly file: " + fileName; + LOG_ERROR("%s\n", m_lastError.c_str()); + polyLines.clear(); + return ERR_CODE(FILE_ERR_READ); + } + + inputFile.close(); + LOG_INFO("Loaded %zu poly lines from file: %s\n", polyLines.size(), fileName.c_str()); + return SUCCESS; +} + +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: @@ -869,298 +966,303 @@ void LaserDataLoader::FreeLaserScanData(std::vector>格式 -int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector>& unifiedData, - std::vector>& scanLines, - size_t* validPointCount) -{ - LOG_DEBUG("Converting unified data to scan lines format\n"); - - scanLines.clear(); - - // 初始化有效点计数 - size_t validCount = 0; - - // 浮点数零值判断阈值 - const double EPSILON = 1e-6; - - for (const auto& linePair : unifiedData) { - EVzResultDataType dataType = linePair.first; - const SVzLaserLineData& lineData = linePair.second; - - // 只处理Position类型的数据 - if (dataType == keResultDataType_Position && lineData.p3DPoint) { - std::vector scanLine; - - // 为当前扫描线分配空间 - scanLine.reserve(lineData.nPointCount); - - // 复制点数据并统计有效点 - const SVzNL3DPosition* points = static_cast(lineData.p3DPoint); - for (int i = 0; i < lineData.nPointCount; i++) { - scanLine.push_back(points[i]); - - // 统计有效点（xyz全非0，使用epsilon阈值判断） - if (std::fabs(points[i].pt3D.x) > EPSILON && - std::fabs(points[i].pt3D.y) > EPSILON && - std::fabs(points[i].pt3D.z) > EPSILON) { - validCount++; - } - } - - scanLines.push_back(scanLine); - } - } - - // 返回有效点计数（如果调用者需要） - if (validPointCount) { - *validPointCount = validCount; - } - - LOG_DEBUG("Converted %zu lines to scan lines format, valid points: %zu\n", scanLines.size(), validCount); - return SUCCESS; -} - -// 转换统一格式数据为SVzNL3DLaserLine格式 -int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector>& unifiedData, - std::vector& xyzData) -{ - xyzData.clear(); - - for (const auto& linePair : unifiedData) { - EVzResultDataType dataType = linePair.first; - const SVzLaserLineData& lineData = linePair.second; - - // 只处理Position类型的数据 - if (dataType == keResultDataType_Position && lineData.p3DPoint) { - SVzNL3DLaserLine xyzLine; - xyzLine.nTimeStamp = lineData.llTimeStamp; - xyzLine.nPositionCnt = lineData.nPointCount; - - if (lineData.nPointCount > 0) { - xyzLine.p3DPosition = new SVzNL3DPosition[lineData.nPointCount]; - if (xyzLine.p3DPosition) { - memcpy(xyzLine.p3DPosition, lineData.p3DPoint, sizeof(SVzNL3DPosition) * lineData.nPointCount); - } else { - m_lastError = "Memory allocation failed for SVzNL3DPosition"; - LOG_ERROR("Memory allocation failed for SVzNL3DPosition\n"); - return ERR_CODE(DATA_ERR_INVALID); - } - } else { - xyzLine.p3DPosition = nullptr; - } - - xyzData.push_back(xyzLine); - } - } - - LOG_DEBUG("Converted %zu lines to SVzNL3DLaserLine format\n", xyzData.size()); - return SUCCESS; -} - -// 转换统一格式数据为SVzNLXYZRGBDLaserLine格式 -int LaserDataLoader::ConvertToSVzNLXYZRGBDLaserLine(const std::vector>& 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) { - 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, Y, Z; - float leftX, leftY; - float rightX, rightY; - sscanf(data.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); - sData.pt3D.x = X; - sData.pt3D.y = Y; - sData.pt3D.z = Z; - s2DData.ptLeft2D.x = leftX; - s2DData.ptLeft2D.y = leftY; - s2DData.ptRight2D.x = rightX; - s2DData.ptRight2D.y = rightY; - return SUCCESS; -} - -int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNLPointXYZRGBA& sData, SVzNL2DLRPoint& s2DData) -{ - float X, Y, Z; - float leftX, leftY; - float rightX, rightY; - - // 尝试新格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A} - int R, G, B, A; - int parsed = sscanf(data.c_str(), - " { %f , %f , %f } - { %f , %f } - { %f , %f } - { %d , %d , %d , %d }", - &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY, &R, &G, &B, &A); - if (parsed >= 11) { - sData.x = X; - sData.y = Y; - sData.z = Z; - - // R,G,B,A 是 0-255 整数值，Alpha 存储在高 8 位 - unsigned int nRGB = static_cast(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, g, b; - sscanf(data.c_str(), - " { %f , %f , %f , %f , %f , %f } - { %f , %f } - { %f , %f }", - &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); - sData.x = X; - sData.y = Y; - sData.z = Z; - - int nr = (int)(r * 255); - int ng = (int)(g * 255); - int nb = (int)(b * 255); - nb <<= 8; - nb += ng; - nb <<= 8; - nb += nr; - sData.nRGB = nb; - - s2DData.sLeft.x = leftX; - s2DData.sLeft.y = leftY; - s2DData.sRight.x = rightX; - s2DData.sRight.y = rightY; - return SUCCESS; -} - -// 获取激光数据类型 -int LaserDataLoader::_GetLaserType(const std::string& fileName, EVzResultDataType& eDataType) -{ - std::ifstream inputFile(fileName); - std::string linedata; - - if (!inputFile.is_open()) { - m_lastError = "Cannot open file: " + fileName; - return ERR_CODE(FILE_ERR_NOEXIST); - } - - bool bFind = false; - while (std::getline(inputFile, linedata)) { - TrimCarriageReturn(linedata); - - if (linedata.find("{") == 0) { - // 统计 { 的数量判断格式 - int braceCount = 0; - for (char c : linedata) { - if (c == '{') braceCount++; - } - - if (braceCount == 4) { - // 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A} - eDataType = keResultDataType_PointXYZRGBA; - bFind = true; - } else if (braceCount == 3) { - // 检查第一组的逗号数量区分XYZ和旧RGBD - size_t firstClose = linedata.find('}'); - if (firstClose != std::string::npos) { - int commaCount = 0; - for (size_t i = 0; i < firstClose; ++i) { - if (linedata[i] == ',') commaCount++; - } - if (commaCount >= 5) { - // 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry} - eDataType = keResultDataType_PointXYZRGBA; - bFind = true; - } else if (commaCount >= 2) { - // XYZ格式: {x,y,z}-{lx,ly}-{rx,ry} - eDataType = keResultDataType_Position; - bFind = true; - } - } - } - break; - } - } - - inputFile.close(); - return bFind ? SUCCESS : ERR_CODE(FILE_ERR_FORMAT); -} + } + 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) +{ + LOG_DEBUG("Converting unified data to scan lines format\n"); + + scanLines.clear(); + + // 初始化有效点计数 + size_t validCount = 0; + + // 浮点数零值判断阈值 + const double EPSILON = 1e-6; + + for (const auto& linePair : unifiedData) { + EVzResultDataType dataType = linePair.first; + const SVzLaserLineData& lineData = linePair.second; + + // 只处理Position类型的数据 + if (dataType == keResultDataType_Position && lineData.p3DPoint) { + std::vector scanLine; + + // 为当前扫描线分配空间 + scanLine.reserve(lineData.nPointCount); + + // 复制点数据并统计有效点 + const SVzNL3DPosition* points = static_cast(lineData.p3DPoint); + for (int i = 0; i < lineData.nPointCount; i++) { + scanLine.push_back(points[i]); + + // 统计有效点（xyz全非0，使用epsilon阈值判断） + if (std::fabs(points[i].pt3D.x) > EPSILON && + std::fabs(points[i].pt3D.y) > EPSILON && + std::fabs(points[i].pt3D.z) > EPSILON) { + validCount++; + } + } + + scanLines.push_back(scanLine); + } + } + + // 返回有效点计数（如果调用者需要） + if (validPointCount) { + *validPointCount = validCount; + } + + LOG_DEBUG("Converted %zu lines to scan lines format, valid points: %zu\n", scanLines.size(), validCount); + return SUCCESS; +} + +// 转换统一格式数据为SVzNL3DLaserLine格式 +int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector>& unifiedData, + std::vector& xyzData) +{ + xyzData.clear(); + + for (const auto& linePair : unifiedData) { + EVzResultDataType dataType = linePair.first; + const SVzLaserLineData& lineData = linePair.second; + + // 只处理Position类型的数据 + if (dataType == keResultDataType_Position && lineData.p3DPoint) { + SVzNL3DLaserLine xyzLine; + xyzLine.nTimeStamp = lineData.llTimeStamp; + xyzLine.nPositionCnt = lineData.nPointCount; + + if (lineData.nPointCount > 0) { + xyzLine.p3DPosition = new SVzNL3DPosition[lineData.nPointCount]; + if (xyzLine.p3DPosition) { + memcpy(xyzLine.p3DPosition, lineData.p3DPoint, sizeof(SVzNL3DPosition) * lineData.nPointCount); + } else { + m_lastError = "Memory allocation failed for SVzNL3DPosition"; + LOG_ERROR("Memory allocation failed for SVzNL3DPosition\n"); + return ERR_CODE(DATA_ERR_INVALID); + } + } else { + xyzLine.p3DPosition = nullptr; + } + + xyzData.push_back(xyzLine); + } + } + + LOG_DEBUG("Converted %zu lines to SVzNL3DLaserLine format\n", xyzData.size()); + return SUCCESS; +} + +// 转换统一格式数据为SVzNLXYZRGBDLaserLine格式 +int LaserDataLoader::ConvertToSVzNLXYZRGBDLaserLine(const std::vector>& 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) { + 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; + if (7 != sscanf(data.c_str(), " { %f , %f , %f } - { %f , %f } - { %f , %f }", + &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY)) { + 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) { + // XYZ格式: {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/CloudView/CloudView.pro b/CloudView/CloudView.pro index adcf38a..e38b636 100644 --- a/CloudView/CloudView.pro +++ b/CloudView/CloudView.pro @@ -15,9 +15,9 @@ TEMPLATE = app # 椤圭洰鐩綍 INCLUDEPATH += $$PWD/Inc +INCLUDEPATH += $$PWD/../../Device/SDK/VzNLSDK/Inc INCLUDEPATH += $$PWD/../CloudView3D/Inc INCLUDEPATH += $$PWD/../CloudUtils/Inc -INCLUDEPATH += $$PWD/../../Device/SDK/VzNLSDK/Inc INCLUDEPATH += $$PWD/../VrUtils/Inc INCLUDEPATH += $$PWD/../VrCommon/Inc @@ -35,23 +35,28 @@ SOURCES += \ # 璧勬簮鏂囦欢 RESOURCES += resource/resource.qrc -RESOURCES += $$PWD/../../AppUtils/UICommon/common_resources.qrc +COMMON_RESOURCES = $$PWD/../../AppUtils/UICommon/common_resources.qrc +exists($$COMMON_RESOURCES) { + RESOURCES += $$COMMON_RESOURCES +} else { + message("CloudView: optional common_resources.qrc not found; using text view buttons") +} # Windows 搴旂敤鍥炬爣 RC_ICONS = resource/logo.ico # Windows骞冲彴搴撻摼鎺? win32:CONFIG(release, debug|release): { - LIBS += -L../CloudView3D/release -lCloudView3D - LIBS += -L../CloudUtils/release -lCloudUtils - LIBS += -L../VrUtils/release -lVrUtils - LIBS += -L../VrCommon/release -lVrCommon + LIBS += -L$$PWD/../CloudView3D/release -lCloudView3D + LIBS += -L$$PWD/../CloudUtils/release -lCloudUtils + LIBS += -L$$PWD/../VrUtils/release -lVrUtils + LIBS += -L$$PWD/../VrCommon/release -lVrCommon } else:win32:CONFIG(debug, debug|release): { - LIBS += -L../CloudView3D/debug -lCloudView3D - LIBS += -L../CloudUtils/debug -lCloudUtils - LIBS += -L../VrUtils/debug -lVrUtils - LIBS += -L../VrCommon/debug -lVrCommon + LIBS += -L$$PWD/../CloudView3D/debug -lCloudView3D + LIBS += -L$$PWD/../CloudUtils/debug -lCloudUtils + LIBS += -L$$PWD/../VrUtils/debug -lVrUtils + LIBS += -L$$PWD/../VrCommon/debug -lVrCommon } # Windows绯荤粺搴? diff --git a/CloudView/Src/CloudViewMainWindow.cpp b/CloudView/Src/CloudViewMainWindow.cpp index 9dd45d8..4b0ccfa 100644 --- a/CloudView/Src/CloudViewMainWindow.cpp +++ b/CloudView/Src/CloudViewMainWindow.cpp @@ -1357,32 +1357,38 @@ QWidget* CloudViewMainWindow::createViewToolbar() layout->setSpacing(3); // 视图预设：图标、提示、旋转角度 - struct ViewPreset { - const char* iconPath; - const char* tooltip; + struct ViewPreset { + const char* iconPath; + const char* fallbackText; + const char* tooltip; float rotX; float rotY; float rotZ; }; - // 坐标系定义：X向右，Y向下，Z朝后 - ViewPreset presets[] = { - {":/common/resource/view_front.png", "正视 (XY面)", 180.0f, 0.0f, 0.0f}, - {":/common/resource/view_back.png", "后视", 180.0f, 180.0f, 0.0f}, - {":/common/resource/view_left.png", "左视 (YZ面)", 180.0f, 90.0f, 0.0f}, - {":/common/resource/view_right.png", "右视", 180.0f, -90.0f, 0.0f}, - {":/common/resource/view_top.png", "俯视 (XZ面)", 90.0f, 0.0f, 0.0f}, - {":/common/resource/view_bottom.png", "仰视", -90.0f, 0.0f, 0.0f}, - {":/common/resource/view_robot.png", "机械臂", -90.0f, 90.0f, 0.0f}, - }; + // 坐标系定义：X向右，Y向下，Z朝后 + ViewPreset presets[] = { + {":/common/resource/view_front.png", "前", "正视 (XY面)", 180.0f, 0.0f, 0.0f}, + {":/common/resource/view_back.png", "后", "后视", 180.0f, 180.0f, 0.0f}, + {":/common/resource/view_left.png", "左", "左视 (YZ面)", 180.0f, 90.0f, 0.0f}, + {":/common/resource/view_right.png", "右", "右视", 180.0f, -90.0f, 0.0f}, + {":/common/resource/view_top.png", "上", "俯视 (XZ面)", 90.0f, 0.0f, 0.0f}, + {":/common/resource/view_bottom.png", "下", "仰视", -90.0f, 0.0f, 0.0f}, + {":/common/resource/view_robot.png", "机", "机械臂", -90.0f, 90.0f, 0.0f}, + }; int btnSize = 32; int iconSize = 24; - for (const auto& preset : presets) { - QToolButton* btn = new QToolButton(widget); - btn->setIcon(QIcon(preset.iconPath)); - btn->setIconSize(QSize(iconSize, iconSize)); + for (const auto& preset : presets) { + QToolButton* btn = new QToolButton(widget); + const QIcon icon(preset.iconPath); + if (icon.isNull()) { + btn->setText(QString::fromUtf8(preset.fallbackText)); + } else { + btn->setIcon(icon); + btn->setIconSize(QSize(iconSize, iconSize)); + } btn->setFixedSize(btnSize, btnSize); btn->setToolTip(preset.tooltip); btn->setAutoRaise(true); @@ -2379,6 +2385,7 @@ void CloudViewMainWindow::showBasicShape(const QString& shapeName, if (!points.isEmpty()) { m_glWidget->appendBasicShapePoints(points); } + m_glWidget->fitToContent(); statusBar()->showMessage(QString("已添加 %1").arg(shapeName)); LOG_INFO("[CloudView] Add basic shape: %s, segments=%d, points=%d\n", shapeName.toStdString().c_str(), segments.size(), points.size()); @@ -2392,6 +2399,7 @@ void CloudViewMainWindow::showBasicSurface(const QString& shapeName, const QVect m_glWidget->appendBasicShapeSurfaces(surfaces); m_glWidget->appendBasicShapeSegments(outlines); + m_glWidget->fitToContent(); statusBar()->showMessage(QString("已添加 %1").arg(shapeName)); LOG_INFO("[CloudView] Add basic surface: %s, surfaces=%d, outlines=%d\n", shapeName.toStdString().c_str(), surfaces.size(), outlines.size()); diff --git a/CloudView/main.cpp b/CloudView/main.cpp index 2229ffc..cf0e6a9 100644 --- a/CloudView/main.cpp +++ b/CloudView/main.cpp @@ -65,6 +65,14 @@ int main(int argc, char* argv[]) 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; diff --git a/CloudView3D/CloudView3D.pro b/CloudView3D/CloudView3D.pro index 514a324..a88e3ae 100644 --- a/CloudView3D/CloudView3D.pro +++ b/CloudView3D/CloudView3D.pro @@ -4,6 +4,13 @@ CONFIG += c++17 staticlib TEMPLATE = lib TARGET = CloudView3D +win32:CONFIG(debug, debug|release) { + DESTDIR = $$PWD/debug +} +win32:CONFIG(release, debug|release) { + DESTDIR = $$PWD/release +} + win32-msvc { QMAKE_CXXFLAGS += /utf-8 QMAKE_CXXFLAGS += /bigobj @@ -11,8 +18,8 @@ win32-msvc { INCLUDEPATH += $$PWD/Inc INCLUDEPATH += $$PWD/../VrUtils/Inc -INCLUDEPATH += $$PWD/../VrCommon/Inc INCLUDEPATH += $$PWD/../../Device/SDK/VzNLSDK/Inc +INCLUDEPATH += $$PWD/../VrCommon/Inc HEADERS += \ Inc/CloudView3D.h \ diff --git a/CloudView3D/Inc/PointCloudGLWidget.h b/CloudView3D/Inc/PointCloudGLWidget.h index f974fa4..1aaf7ca 100644 --- a/CloudView3D/Inc/PointCloudGLWidget.h +++ b/CloudView3D/Inc/PointCloudGLWidget.h @@ -168,9 +168,10 @@ public: explicit PointCloudGLWidget(QWidget* parent = nullptr); ~PointCloudGLWidget() override; - void addPointCloud(const PointCloudXYZ& cloud, const QString& name = ""); - void addPointCloud(const PointCloudXYZRGB& cloud, const QString& name = ""); - void clearPointClouds(); + void addPointCloud(const PointCloudXYZ& cloud, const QString& name = ""); + void addPointCloud(const PointCloudXYZRGB& cloud, const QString& name = ""); + void appendPointCloud(const PointCloudXYZRGB& cloud, const QString& name = ""); + void clearPointClouds(); void setPointCloudColor(PointCloudColor color); void setPointSize(float size); void resetView(); @@ -454,8 +455,10 @@ private: PointCloudData& operator=(const PointCloudData&) = delete; }; - void computeBoundingBox(); - void setCurrentColor(PointCloudColor color); + void computeBoundingBox(); + void updateProjection(const QMatrix4x4& modelView); + float minimumCameraDistance() const; + void setCurrentColor(PointCloudColor color); void setColorByIndex(int colorIndex); // 根据索引设置颜色 SelectedPointInfo pickPoint(int screenX, int screenY); void drawSelectedPoints(); diff --git a/CloudView3D/Src/PointCloudGLWidget.cpp b/CloudView3D/Src/PointCloudGLWidget.cpp index fb56b81..cf1e503 100644 --- a/CloudView3D/Src/PointCloudGLWidget.cpp +++ b/CloudView3D/Src/PointCloudGLWidget.cpp @@ -1,8 +1,9 @@ #include "PointCloudGLWidget.h" -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include "VrLog.h" @@ -113,16 +114,10 @@ void PointCloudGLWidget::initializeGL() glHint(GL_POINT_SMOOTH_HINT, GL_NICEST); } -void PointCloudGLWidget::resizeGL(int w, int h) -{ - glViewport(0, 0, w, h); - - m_projection.setToIdentity(); - float aspect = static_cast(w) / static_cast(h > 0 ? h : 1); - // 放大裁剪范围以容纳激光雷达点云（最大可能跨越数十万 mm） - // near 取 1.0 保留 depth buffer 精度，far 放到 1e7 避免远点被裁掉 - m_projection.perspective(45.0f, aspect, 1.0f, 1.0e7f); -} +void PointCloudGLWidget::resizeGL(int w, int h) +{ + glViewport(0, 0, w, h); +} void PointCloudGLWidget::paintGL() { @@ -146,12 +141,14 @@ void PointCloudGLWidget::paintGL() euler.x(), euler.y(), euler.z()); } - glMatrixMode(GL_PROJECTION); - glLoadMatrixf(m_projection.constData()); - - glMatrixMode(GL_MODELVIEW); - QMatrix4x4 modelView = m_view * m_model; - glLoadMatrixf(modelView.constData()); + QMatrix4x4 modelView = m_view * m_model; + + glMatrixMode(GL_PROJECTION); + updateProjection(modelView); + glLoadMatrixf(m_projection.constData()); + + glMatrixMode(GL_MODELVIEW); + glLoadMatrixf(modelView.constData()); // 确保颜色数组禁用 glDisableClientState(GL_COLOR_ARRAY); @@ -330,8 +327,8 @@ void PointCloudGLWidget::addPointCloud(const PointCloudXYZ& cloud, const QString update(); } -void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QString& name) -{ +void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QString& name) +{ if (cloud.empty()) { return; } @@ -437,10 +434,105 @@ void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QStr computeBoundingBox(); resetView(); - update(); -} - -void PointCloudGLWidget::clearPointClouds() + update(); +} + +void PointCloudGLWidget::appendPointCloud(const PointCloudXYZRGB& cloud, + const QString& name) +{ + if (cloud.empty()) { + return; + } + if (m_pointClouds.empty() || !m_pointClouds.back().hasColor) { + addPointCloud(cloud, name); + return; + } + + PointCloudData& data = m_pointClouds.back(); + if (!name.isEmpty()) { + data.name = name; + } + + const bool incomingHasLineInfo = !cloud.lineIndices.empty(); + data.hasLineInfo = data.hasLineInfo || incomingHasLineInfo; + const size_t originalIndexOffset = data.vertices.size() / 3; + const float epsilon = 1e-6f; + int previousLineIndex = -1; + int pointInLineIndex = 0; + + for (size_t index = 0; index < cloud.points.size(); ++index) { + const Point3DRGB& point = cloud.points[index]; + int lineIndex = -1; + if (incomingHasLineInfo && index < cloud.lineIndices.size()) { + lineIndex = cloud.lineIndices[index]; + } + if (lineIndex != previousLineIndex) { + pointInLineIndex = 0; + previousLineIndex = lineIndex; + } + + if (!std::isfinite(point.x) || !std::isfinite(point.y) || + !std::isfinite(point.z) || + (std::fabs(point.x) < epsilon && + std::fabs(point.y) < epsilon && + std::fabs(point.z) < epsilon)) { + ++pointInLineIndex; + continue; + } + + const size_t pointIndex = data.vertices.size() / 3; + data.vertices.push_back(point.x); + data.vertices.push_back(point.y); + data.vertices.push_back(point.z); + data.colors.push_back(point.r / 255.0f); + data.colors.push_back(point.g / 255.0f); + data.colors.push_back(point.b / 255.0f); + data.originalIndices.push_back( + static_cast(originalIndexOffset + index)); + data.pointInLineIndices.push_back(pointInLineIndex); + + if (lineIndex >= 0) { + data.lineIndices.push_back(lineIndex); + data.totalLines = qMax(data.totalLines, lineIndex + 1); + } + + if (point.pointSize > 1.0f) { + auto sizeGroup = std::find_if( + data.customPointSizeGroups.begin(), + data.customPointSizeGroups.end(), + [&point](const PointCloudData::PointSizeGroup& group) { + return group.pointSize == point.pointSize; + }); + if (sizeGroup == data.customPointSizeGroups.end()) { + PointCloudData::PointSizeGroup group; + group.pointSize = point.pointSize; + data.customPointSizeGroups.push_back(std::move(group)); + sizeGroup = data.customPointSizeGroups.end() - 1; + } + sizeGroup->indices.push_back(pointIndex); + data.hasCustomPointSizes = true; + } + ++pointInLineIndex; + } + + if (data.totalLines > 0) { + data.pointsPerLine = + static_cast(data.vertices.size() / 3) / data.totalLines; + } + + makeCurrent(); + uploadToVBO(data); + doneCurrent(); + + computeBoundingBox(); + const QVector3D size = m_maxBound - m_minBound; + const float maximumSize = qMax(qMax(size.x(), size.y()), size.z()); + m_distance = qMax(m_distance, + qBound(10.0f, maximumSize * 2.5f, 1.0e7f)); + update(); +} + +void PointCloudGLWidget::clearPointClouds() { // 释放所有 VBO makeCurrent(); @@ -506,19 +598,83 @@ void PointCloudGLWidget::fitToContent() resetView(); } +void PointCloudGLWidget::updateProjection(const QMatrix4x4& modelView) +{ + const QVector3D corners[] = { + {m_minBound.x(), m_minBound.y(), m_minBound.z()}, + {m_minBound.x(), m_minBound.y(), m_maxBound.z()}, + {m_minBound.x(), m_maxBound.y(), m_minBound.z()}, + {m_minBound.x(), m_maxBound.y(), m_maxBound.z()}, + {m_maxBound.x(), m_minBound.y(), m_minBound.z()}, + {m_maxBound.x(), m_minBound.y(), m_maxBound.z()}, + {m_maxBound.x(), m_maxBound.y(), m_minBound.z()}, + {m_maxBound.x(), m_maxBound.y(), m_maxBound.z()} + }; + + float minimumDepth = FLT_MAX; + float maximumDepth = -FLT_MAX; + for (const QVector3D& corner : corners) { + const float depth = -modelView.map(corner).z(); + minimumDepth = qMin(minimumDepth, depth); + maximumDepth = qMax(maximumDepth, depth); + } + + constexpr float kMinimumNearPlane = 0.01f; + float nearPlane = kMinimumNearPlane; + float farPlane = 1.0f; + if (maximumDepth > kMinimumNearPlane) { + // 点云贴近或跨过相机时保持很小的近裁剪距离，避免前侧点被裁掉。 + if (minimumDepth > kMinimumNearPlane) { + nearPlane = qMax(kMinimumNearPlane, minimumDepth * 0.5f); + } + // 远裁剪面仅覆盖当前内容，并限制 near/far 比值以保证深度缓冲精度。 + farPlane = qMax(nearPlane + 1.0f, maximumDepth * 1.5f); + nearPlane = qMax(nearPlane, farPlane / 100000.0f); + } + + const float aspect = static_cast(width()) / + static_cast(height() > 0 ? height() : 1); + m_projection.setToIdentity(); + m_projection.perspective(45.0f, aspect, nearPlane, farPlane); +} + +float PointCloudGLWidget::minimumCameraDistance() const +{ + const QVector3D corners[] = { + {m_minBound.x(), m_minBound.y(), m_minBound.z()}, + {m_minBound.x(), m_minBound.y(), m_maxBound.z()}, + {m_minBound.x(), m_maxBound.y(), m_minBound.z()}, + {m_minBound.x(), m_maxBound.y(), m_maxBound.z()}, + {m_maxBound.x(), m_minBound.y(), m_minBound.z()}, + {m_maxBound.x(), m_minBound.y(), m_maxBound.z()}, + {m_maxBound.x(), m_maxBound.y(), m_minBound.z()}, + {m_maxBound.x(), m_maxBound.y(), m_maxBound.z()} + }; + + float frontExtent = 0.0f; + for (const QVector3D& corner : corners) { + const QVector3D relativePosition = corner - m_center; + frontExtent = qMax(frontExtent, + m_rotation.rotatedVector(relativePosition).z()); + } + + const float margin = qMax(0.1f, (m_maxBound - m_minBound).length() * 0.002f); + return qBound(0.1f, frontExtent + margin, 1.0e7f); +} + void PointCloudGLWidget::zoomIn() { m_distance *= 0.9f; - m_distance = qBound(0.1f, m_distance, 1.0e7f); - update(); -} + m_distance = qBound(minimumCameraDistance(), m_distance, 1.0e7f); + update(); +} void PointCloudGLWidget::zoomOut() -{ - m_distance *= 1.1f; - m_distance = qBound(0.1f, m_distance, 1.0e7f); - update(); -} +{ + m_distance *= 1.1f; + m_distance = qBound(minimumCameraDistance(), m_distance, 1.0e7f); + update(); +} void PointCloudGLWidget::setViewAngles(float rotX, float rotY, float rotZ) { @@ -527,8 +683,9 @@ void PointCloudGLWidget::setViewAngles(float rotX, float rotY, float rotZ) m_rotationZ = rotZ; m_pan = QVector3D(0, 0, 0); - // 从欧拉角转换为四元数（ZYX顺序） - m_rotation = QQuaternion::fromEulerAngles(rotX, rotY, rotZ); + // 从欧拉角转换为四元数（ZYX顺序） + m_rotation = QQuaternion::fromEulerAngles(rotX, rotY, rotZ); + m_distance = qMax(m_distance, minimumCameraDistance()); LOG_INFO("[CloudView] setViewAngles: rotX=%.1f, rotY=%.1f, rotZ=%.1f\n", rotX, rotY, rotZ); @@ -1440,8 +1597,9 @@ void PointCloudGLWidget::mouseMoveEvent(QMouseEvent* event) m_rotationY += angleY; } - // 发送角度变化信号 - emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ); + // 发送角度变化信号 + m_distance = qMax(m_distance, minimumCameraDistance()); + emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ); update(); } else if (m_middleButtonPressed) { float factor = m_distance * 0.002f; @@ -1452,9 +1610,10 @@ void PointCloudGLWidget::mouseMoveEvent(QMouseEvent* event) // 右键拖动：绕视线方向旋转（滚转）（修正方向） float angle = -delta.x() * 0.5f; // 取反修正方向 QQuaternion deltaRotation = QQuaternion::fromAxisAndAngle(QVector3D(0, 0, 1), angle); - m_rotation = deltaRotation * m_rotation; - m_rotationZ += angle; - emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ); + m_rotation = deltaRotation * m_rotation; + m_rotationZ += angle; + m_distance = qMax(m_distance, minimumCameraDistance()); + emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ); update(); } } @@ -1472,11 +1631,11 @@ void PointCloudGLWidget::mouseReleaseEvent(QMouseEvent* event) void PointCloudGLWidget::wheelEvent(QWheelEvent* event) { - float delta = event->angleDelta().y() / 120.0f; - m_distance *= (1.0f - delta * 0.1f); - m_distance = qBound(0.1f, m_distance, 1.0e7f); - update(); -} + float delta = event->angleDelta().y() / 120.0f; + m_distance *= (1.0f - delta * 0.1f); + m_distance = qBound(minimumCameraDistance(), m_distance, 1.0e7f); + update(); +} void PointCloudGLWidget::keyPressEvent(QKeyEvent* event) { diff --git a/VrCommon/VrCommon.pro b/VrCommon/VrCommon.pro index f985437..360462b 100644 --- a/VrCommon/VrCommon.pro +++ b/VrCommon/VrCommon.pro @@ -1,10 +1,17 @@ #CONFIG -= qt -TEMPLATE = lib -CONFIG += staticlib -CONFIG += c++17 - -# You can make your code fail to compile if it uses deprecated APIs. +TEMPLATE = lib +CONFIG += staticlib +CONFIG += c++17 + +win32:CONFIG(debug, debug|release) { + DESTDIR = $$PWD/debug +} +win32:CONFIG(release, debug|release) { + DESTDIR = $$PWD/release +} + +# You can make your code fail to compile if it uses deprecated APIs. # In order to do so, uncomment the following line. #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 diff --git a/VrUtils/VrUtils.pro b/VrUtils/VrUtils.pro index b77d3d5..3f1220b 100644 --- a/VrUtils/VrUtils.pro +++ b/VrUtils/VrUtils.pro @@ -6,9 +6,16 @@ DEFINES += TINYXML2_EXPORT DEFINES += HAVE_SNPRINTF DEFINES += LOG4CPP_HAVE_SSTREAM=1 DEFINES += VR_UTILS_EXPORTS -CONFIG += c++11 - -# Unix平台特定配置 +CONFIG += c++11 + +win32:CONFIG(debug, debug|release) { + DESTDIR = $$PWD/debug +} +win32:CONFIG(release, debug|release) { + DESTDIR = $$PWD/release +} + +# Unix平台特定配置 unix:!macx { DEFINES += LOG4CPP_HAVE_UNISTD_H # 确保所有符号导出（对于静态库）