diff --git a/CloudUtils/Inc/LaserDataLoader.h b/CloudUtils/Inc/LaserDataLoader.h index dd692bf..24cee2f 100644 --- a/CloudUtils/Inc/LaserDataLoader.h +++ b/CloudUtils/Inc/LaserDataLoader.h @@ -63,23 +63,28 @@ public: // 转换统一格式数据为std::vector>格式 // validPointCount: 可选输出参数，返回有效点个数（xyz全非0的点） - int ConvertToSVzNL3DPosition(const std::vector>& unifiedData, - std::vector>& scanLines, - size_t* validPointCount = nullptr); - - // 从文件加载Poly折线数据（带颜色） - // polyLines: 每条折线的点坐标列表，polyLines[i] 是第i条折线的所有点（含RGB颜色） - int LoadPolySegments(const std::string& fileName, - std::vector>& polyLines); + int ConvertToSVzNL3DPosition(const std::vector>& unifiedData, + std::vector>& scanLines, + size_t* validPointCount = nullptr); + + // 转换统一格式数据为std::vector>格式 + int ConvertToSVzNLPositionD(const std::vector>& unifiedData, + std::vector>& scanLines); + + // 从文件加载Poly折线数据（带颜色） + // polyLines: 每条折线的点坐标列表，polyLines[i] 是第i条折线的所有点（含RGB颜色） + int LoadPolySegments(const std::string& fileName, + std::vector>& polyLines); // 获取最后的错误信息 std::string GetLastError() const { return m_lastError; } private: - // 读取XYZ格式的激光数据 - int _ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPosition& s2DData); - - // 读取RGBD格式的激光数据 - 新增RGBD支持 + // 读取XYZ格式的激光数据 + int _ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPosition& s2DData); + int _ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPositionF& s2DData); + + // 读取RGBD格式的激光数据 - 新增RGBD支持 int _ParseLaserScanPoint(const std::string& data, SVzNLPointXYZRGBA& sData, SVzNL2DLRPoint& s2DData); // 获取激光数据类型 diff --git a/CloudUtils/Src/LaserDataLoader.cpp b/CloudUtils/Src/LaserDataLoader.cpp index d7537e2..052be43 100644 --- a/CloudUtils/Src/LaserDataLoader.cpp +++ b/CloudUtils/Src/LaserDataLoader.cpp @@ -535,19 +535,6 @@ int LaserDataLoader::SaveLaserScanData(const std::string& fileName, 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); @@ -587,13 +574,30 @@ 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, - leftX, leftY, rightX, rightY); + 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())); } @@ -633,15 +637,12 @@ 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, - leftX, leftY, rightX, rightY); + points2D[i].sLeft.x, points2D[i].sLeft.y, + points2D[i].sRight.x, points2D[i].sRight.y); } sw.write(buffer.data(), static_cast(buffer.size())); } @@ -917,7 +918,7 @@ void LaserDataLoader::FreeLaserScanData(std::vector(lineData.p3DPoint); break; case keResultDataType_PositionF: - delete[] static_cast(lineData.p3DPoint); + delete[] static_cast(lineData.p3DPoint); break; case keResultDataType_PointF: delete[] static_cast(lineData.p3DPoint); @@ -1012,6 +1013,22 @@ int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector scanLine; + scanLine.reserve(lineData.nPointCount); + + const SVzNL3DPosition* points = static_cast(lineData.p3DPoint); + for (int i = 0; i < lineData.nPointCount; i++) { + scanLine.push_back(points[i]); + + if (std::fabs(points[i].pt3D.x) > EPSILON && + std::fabs(points[i].pt3D.y) > EPSILON && + std::fabs(points[i].pt3D.z) > EPSILON) { + validCount++; + } + } + scanLines.push_back(scanLine); } } @@ -1025,6 +1042,82 @@ int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector>& unifiedData, + std::vector>& scanLines) +{ + LOG_DEBUG("Converting unified data to SVzNLPositionD scan lines format\n"); + + scanLines.clear(); + + for (const auto& linePair : unifiedData) { + if (linePair.first != keResultDataType_Position && + linePair.first != keResultDataType_PositionF) { + continue; + } + + const SVzLaserLineData& lineData = linePair.second; + if (lineData.nPointCount < 0) { + continue; + } + + if (lineData.nPointCount > 0 && (!lineData.p3DPoint || !lineData.p2DPoint)) { + m_lastError = "Missing 3D or 2D point buffer in Position laser data"; + LOG_ERROR("%s, pointCount=%d, p3D=%p, p2D=%p\n", + m_lastError.c_str(), + lineData.nPointCount, + lineData.p3DPoint, + lineData.p2DPoint); + continue; + } + + std::vector scanLine; + scanLine.reserve(static_cast(lineData.nPointCount)); + if (linePair.first == keResultDataType_Position) { + const SVzNL3DPosition* points3D = static_cast(lineData.p3DPoint); + const SVzNL2DPosition* points2D = static_cast(lineData.p2DPoint); + for (int i = 0; i < lineData.nPointCount; ++i) { + SVzNLPositionD point; + std::memset(&point, 0, sizeof(point)); + point.nPointIdx = points3D[i].nPointIdx; + point.pt3D = points3D[i].pt3D; + point.ptLeft2D.x = points2D[i].ptLeft2D.x; + point.ptLeft2D.y = points2D[i].ptLeft2D.y; + point.ptRight2D.x = points2D[i].ptRight2D.x; + point.ptRight2D.y = points2D[i].ptRight2D.y; + scanLine.push_back(point); + } + } else { + const SVzNL3DPosition* points3D = static_cast(lineData.p3DPoint); + const SVzNL2DPositionF* points2D = static_cast(lineData.p2DPoint); + for (int i = 0; i < lineData.nPointCount; ++i) { + SVzNLPositionD point; + std::memset(&point, 0, sizeof(point)); + point.nPointIdx = points3D[i].nPointIdx; + point.pt3D = points3D[i].pt3D; + point.ptLeft2D.x = points2D[i].ptLeft2D.x; + point.ptLeft2D.y = points2D[i].ptLeft2D.y; + point.ptRight2D.x = points2D[i].ptRight2D.x; + point.ptRight2D.y = points2D[i].ptRight2D.y; + scanLine.push_back(point); + } + } + + if (!scanLine.empty()) { + scanLines.push_back(scanLine); + } + } + + if (scanLines.empty()) { + m_lastError = "No Position laser data to convert"; + LOG_ERROR("%s\n", m_lastError.c_str()); + return ERR_CODE(DATA_ERR_INVALID); + } + + LOG_DEBUG("Converted %zu lines to SVzNLPositionD scan lines format\n", scanLines.size()); + return SUCCESS; +} + // 转换统一格式数据为SVzNL3DLaserLine格式 int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector>& unifiedData, std::vector& xyzData) @@ -1035,8 +1128,9 @@ int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector= 2) { - // XYZ格式: {x,y,z}-{lx,ly}-{rx,ry} + // XYZUV文本格式: {x,y,z}-{lx,ly}-{rx,ry} eDataType = keResultDataType_Position; bFind = true; } diff --git a/CloudUtils/Src/PointCloudImageUtils.cpp b/CloudUtils/Src/PointCloudImageUtils.cpp index 3e761f0..c7958d1 100644 --- a/CloudUtils/Src/PointCloudImageUtils.cpp +++ b/CloudUtils/Src/PointCloudImageUtils.cpp @@ -1188,10 +1188,11 @@ int PointCloudImageUtils::GenerateDepthImage( if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue; // 根据数据类型处理点云数据 - if (dataType == keResultDataType_Position) { - SVzNL3DPosition* positions = static_cast(lineData.p3DPoint); - for (int i = 0; i < lineData.nPointCount; i++) { - const SVzNL3DPosition& point = positions[i]; + if (dataType == keResultDataType_Position || + dataType == keResultDataType_PositionF) { + SVzNL3DPosition* positions = static_cast(lineData.p3DPoint); + for (int i = 0; i < lineData.nPointCount; i++) { + const SVzNL3DPosition& point = positions[i]; if (point.pt3D.z < 1e-4) continue; if (point.pt3D.x < xMin) xMin = point.pt3D.x; @@ -1250,10 +1251,11 @@ int PointCloudImageUtils::GenerateDepthImage( if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue; - if (dataType == keResultDataType_Position) { - SVzNL3DPosition* positions = static_cast(lineData.p3DPoint); - for (int i = 0; i < lineData.nPointCount; i++) { - const SVzNL3DPosition& point = positions[i]; + if (dataType == keResultDataType_Position || + dataType == keResultDataType_PositionF) { + SVzNL3DPosition* positions = static_cast(lineData.p3DPoint); + for (int i = 0; i < lineData.nPointCount; i++) { + const SVzNL3DPosition& point = positions[i]; if (point.pt3D.z < 1e-4) continue; // 计算图像坐标 diff --git a/Utils.pro b/Utils.pro index 3478236..06060a0 100644 --- a/Utils.pro +++ b/Utils.pro @@ -9,16 +9,11 @@ CloudView3D.file = CloudView3D/CloudView3D.pro CloudView.file = CloudView/CloudView.pro # 添加子项目 -SUBDIRS += \ - VrCommon \ - VrUtils \ - CloudUtils \ - DataUtils - -win32-msvc { - SUBDIRS += \ - CloudView3D \ - CloudView +SUBDIRS += \ + VrCommon \ + VrUtils \ + CloudView3D \ + CloudView } # 设置依赖关系