修正RGBD数据的加载及转换

CloudUtils/Inc/LaserDataLoader.h

@@ -67,6 +67,10 @@ public:
                           std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                           size_t* validPointCount = nullptr);
 
+    // 转换统一格式数据为std::vector<std::vector<SVzNLPositionD>>格式
+    int ConvertToSVzNLPositionD(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
+                                std::vector<std::vector<SVzNLPositionD>>& scanLines);
+
     // 从文件加载Poly折线数据（带颜色）
     // polyLines: 每条折线的点坐标列表，polyLines[i] 是第i条折线的所有点（含RGB颜色）
     int LoadPolySegments(const std::string& fileName,
@@ -78,6 +82,7 @@ public:
 private:
     // 读取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);

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<int>(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<float>(points[i].pt3D.x);
                         float y = static_cast<float>(points[i].pt3D.y);
                         float z = static_cast<float>(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<std::streamsize>(buffer.size()));
+                }
+            } else if (dataType == keResultDataType_PositionF && lineData.p3DPoint) {
+                const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
+                const SVzNL2DPositionF* points2D = static_cast<const SVzNL2DPositionF*>(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<std::streamsize>(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<std::streamsize>(buffer.size()));
                 }
@@ -917,7 +918,7 @@ void LaserDataLoader::FreeLaserScanData(std::vector<std::pair<EVzResultDataType,
                     delete[] static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
                     break;
                 case keResultDataType_PositionF:
-                    delete[] static_cast<SVzNL3DPositionF*>(lineData.p3DPoint);
+                    delete[] static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
                     break;
                 case keResultDataType_PointF:
                     delete[] static_cast<SVzNL3DPointF*>(lineData.p3DPoint);
@@ -1012,6 +1013,22 @@ int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector<std::pair<EVzRes
                 }
             }
 
+            scanLines.push_back(scanLine);
+        } else if (dataType == keResultDataType_PositionF && lineData.p3DPoint) {
+            std::vector<SVzNL3DPosition> scanLine;
+            scanLine.reserve(lineData.nPointCount);
+
+            const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(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<std::pair<EVzRes
     return SUCCESS;
 }
 
+int LaserDataLoader::ConvertToSVzNLPositionD(
+    const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
+    std::vector<std::vector<SVzNLPositionD>>& 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<SVzNLPositionD> scanLine;
+        scanLine.reserve(static_cast<size_t>(lineData.nPointCount));
+        if (linePair.first == keResultDataType_Position) {
+            const SVzNL3DPosition* points3D = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
+            const SVzNL2DPosition* points2D = static_cast<const SVzNL2DPosition*>(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<const SVzNL3DPosition*>(lineData.p3DPoint);
+            const SVzNL2DPositionF* points2D = static_cast<const SVzNL2DPositionF*>(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<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
                                                std::vector<SVzNL3DLaserLine>& xyzData)
@@ -1035,8 +1128,9 @@ int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector<std::pair<EVzRe
         EVzResultDataType dataType = linePair.first;
         const SVzLaserLineData& lineData = linePair.second;
         
-        // 只处理Position类型的数据
+        // PositionF 的 3D buffer 也是 SVzNL3DPosition；仅 2D UV 为浮点。
-        if (dataType == keResultDataType_Position && lineData.p3DPoint) {
+        if ((dataType == keResultDataType_Position ||
+             dataType == keResultDataType_PositionF) && lineData.p3DPoint) {
             SVzNL3DLaserLine xyzLine;
             xyzLine.nTimeStamp = lineData.llTimeStamp;
             xyzLine.nPositionCnt = lineData.nPointCount;
@@ -1143,8 +1237,31 @@ int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPositi
     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 }",
+    const int parsed = sscanf(data.c_str(),
-                    &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY)) {
+        " { %f , %f , %f } - { %f , %f } - { %f , %f }",
+        &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
+    if (parsed != 7) {
+        return ERR_CODE(FILE_ERR_FORMAT);
+    }
+    sData.pt3D.x = X;
+    sData.pt3D.y = Y;
+    sData.pt3D.z = Z;
+    s2DData.ptLeft2D.x = leftX;
+    s2DData.ptLeft2D.y = leftY;
+    s2DData.ptRight2D.x = rightX;
+    s2DData.ptRight2D.y = rightY;
+    return SUCCESS;
+}
+
+int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, 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;
@@ -1253,7 +1370,7 @@ int LaserDataLoader::_GetLaserType(const std::string& fileName, EVzResultDataTyp
                         eDataType = keResultDataType_PointXYZRGBA;
                         bFind = true;
                     } else if (commaCount >= 2) {
-                        // XYZ格式: {x,y,z}-{lx,ly}-{rx,ry}
+                        // XYZUV文本格式: {x,y,z}-{lx,ly}-{rx,ry}
                         eDataType = keResultDataType_Position;
                         bFind = true;
                     }

CloudUtils/Src/PointCloudImageUtils.cpp

@@ -1188,7 +1188,8 @@ int PointCloudImageUtils::GenerateDepthImage(
         if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue;
 
         // 根据数据类型处理点云数据
-        if (dataType == keResultDataType_Position) {
+        if (dataType == keResultDataType_Position ||
+            dataType == keResultDataType_PositionF) {
             SVzNL3DPosition* positions = static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
             for (int i = 0; i < lineData.nPointCount; i++) {
                 const SVzNL3DPosition& point = positions[i];
@@ -1250,7 +1251,8 @@ int PointCloudImageUtils::GenerateDepthImage(
 
         if (!lineData.p3DPoint || lineData.nPointCount <= 0) continue;
 
-        if (dataType == keResultDataType_Position) {
+        if (dataType == keResultDataType_Position ||
+            dataType == keResultDataType_PositionF) {
             SVzNL3DPosition* positions = static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
             for (int i = 0; i < lineData.nPointCount; i++) {
                 const SVzNL3DPosition& point = positions[i];

Utils.pro

@@ -12,11 +12,6 @@ CloudView.file = CloudView/CloudView.pro
 SUBDIRS += \
     VrCommon \
     VrUtils \
-    CloudUtils \
-    DataUtils
-
-win32-msvc {
-    SUBDIRS += \
 	CloudView3D \
 	CloudView
 }